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THE  KINEMATICS  OF  MANIPULATORS  UNDER  COMPUTER  CONTROL 


by  Donald  L.  Pie per 


ABSTRACT:  The  kinematics  of  manipulators  Is  studied.  A  model  is 
presented  which  allows  for  the  systematic  description 
of  new  and  existing  manipulators. 

Six  degree-of-freedom  manipule  ors  are  studied.  Several 
solutions  to  the  problem  of  finding  the  manipulator 
configuration  leading  to  a  specified  position  and  orien¬ 
tation  are  presented.  Numerical  as  well  as  explicit 
solutions  are  given.  The  problem  of  positioning  a  multi- 
link  digital  arm  is  also  discussed. 

Given  the  solution  to  the  position  problem,  a  set  of 
heuristics  is  developed  for  moving  a  six  degree-of- 
freedom  manipulator  from  an  Initial  position  to  a 
final  position  through  a  space  containing  obstacles. 

This  results  In  a  computer  program  shown  to  be  able 
to  direct  a  manipulator  around  obstacles. 


The  research  reported  here  was  supported  in  part  by  the  Advanced  Research 
Projects  Agency  of  the  Office  of  the  Secretary  of  Defense  (SD~183). 


THE  KINEMATICS  OF  MANIPULATORS  UNDER  COMPUTER  CONTROL 


ABSTRACT 

This  dissertation  is  concerned  with  the  kinematic  analysis  of 
computer  controlled  manipulators.  Existing  industrial  and  experimental 
manipulators  are  cataloged  according  to  a  new  model  which  allows  for  the 
systematic  description  of  both  existing  and  new  manipulators. 

This  work  deals  mainly  with  manipulators  consisting  of  six  degree- 
of-freedom  open  chains  of  articulated  links  with  either  turning  (revolute) 
or  sliding  (prismatic)  joints.  The  last  link  called  the  "hand"  is  the 
free  end  of  the  manipulator  and  has  additional  motion  capabilities  which 
make  it  possible  to  grasp  objects. 

The  following  problem  is  discussed:  given  the  desired  hand  position 
and  orientation  along  with  the  various  link  parameters  defining  the 
structure,  what  are  the  values  of  the  manipulator  variables  that  place 
the  hand  at  the  desired  position  with  the  desired  orientation?  Solutions 
to  this  problem  are  presented  for  any  six  degree-of-freedom  manipulator 
with  three  revolute  Joints  whose  axes  intersect  at  a  point,  provided  the 
remaining  three  Joints  are  revolute  or  prismatic  pairs.  These  results 
can  be  expressed  as  a  fourth  degree  polynomial  in  'roe  unknown,  and  closed 
form  expressions  for  the  remaining  unknowns. 

It  is  shown  that  this  is  equivalent  to  the  kinematic  analysis  of  all 
single  loop  five-bar  mechanisms  with  one  sphi  rical  Joint  and  four  Joints 
which  are  revolute  or  prismatic  pairs.  The  extension  to  the  case  where 
only  one  pair  of  axes  intersect  is  discussed.  A  similar  solution  for 
any  manipulator  with  three  prismatic  joints  is  also  given. 


ill 


A  numerical  procedure  based  on  velocity  methods  is  developed  to 
analyze  manipulators  which  cannot  be  "solved"  explicitly.  This  pro¬ 
cedure  is  found  to  be  superior  to  the  widely  used  Newton-Raphson 
technique. 

The  problem  of  positioning  a  "digital  arm"  (i.e.,  a  multi-link 
manipulator  where  each  joint  is  only  capable  of  several  digital  steps) 
is  discussed.  A  simple  searching  algorithm  using  a  look-ahead  scheme 
is  developed.  A  two-dimensional  model  and  three-dimensional  model  are 
studied. 

Given  the  solution  to  the  position  problem,  a  set  of  heuristics  is 
developed  for  moving  a  six  degree-of-freedom  manipulator  from  an  initial 
position  to  a  final  position  through  a  space  containing  obstacles.  A 
mathematical  model  of  objects  is  developed  so  that  possible  conflict 
between  objects  and  any  link  of  the  manipulator  can  be  detected  and 
avoided. 

Some  considerations  in  choosing  a  manipulator  for  use  with  a 
computer  are  discussed.  A  set  of  computer  programs  -  in  FORTRAN  IV  - 
are  developed  to  perform  the  position  analysis  and  trajectory  generations 
for  any  six  degree-of-freedom  manipulator  with  turning  joints. 
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CHAPTER  I 


INTRODUCTION 


Remote  manipulation  involves  having  a  machine  perform  tasks 
requiring  human  dexterity.  Originally,  the  purpose  of  a  manipulator 
was  to  protect  man  from  the  hazards  of  performing  the  work  himself. 

With  the  advance  of  technology,  the  variety  of  tasks  performed  in  hostile 
environments  has  increased.  In  addition  the  scope  of  the  tasks  performed 
by  machines  has  broadened,  so  that  it  is  desirable  for  machines  to  extend 
the  capabilities  of  men  and  to  replace  men  at  tedious  as  well  as  dangerous 
jobs.  Although,  today,  many  processes  and  machines  are  automatically 
controlled,  the  problems  of  remote  manipulation  have  yet  to  be  fully 
solved. 

One  approach  to  this  problem  is  to  use  a  digital  computer  to  control 
a  manipulator.  Then  with  information  obtained  from  visual  as  well  as 
other  sensory  feedback,  the  computer  would  hopefully  be  able  to  direct 
the  manipulator  to  perform  tasks  requiring  some  intelligence  as  well  as 
dexterity. 

This  dissertation  is  concerned  with  the  kinematic  problems  that 
arise  when  a  manipulator  is  subjected  to  computer  control.  These  include 
the  problems  of  position  analysis  and  trajectory  generation. 

In  Chapter  II,  we  discuss  the  classification  and  the  description  of 
manipulators,  including  a  catalog  of  most  of  the  existing  commercial  and 
special  purpose  manipulators. 


The  posicion  problem  is  discussed  in  Chapter  III.  There  we  present 
methods  to  find  values  for  the  manipulator  variables  that  will  place  the 
terminal  device  at  a  given  position. 

In  Chapter  IV,  we  present  numerical  methods  that  may  be  used  to 
analyze  manipulators  too  complex  for  analytic  solution  as  described  in 
Chapter  III. 

The  problems  of  positioning  a  digital  manipulator  are  discussed  in 
Chapter  V. 

Trajectory  generation  -  the  problem  of  moving  a  manipulator  from  a 
given  initial  position  to  a  specified  final  position  -  is  studied  in 
Chapter  VI. 

In  Chapter  VII  we  briefly  discuss  some  considerations  in  choosing 
a  manipulator  for  control  by  computer. 

Chapter  VIII  presents  the  conclusions  and  some  suggestions  for 
future  work. 

In  the  next  section  we  present  a  brief  history  of  remote  manipulation. 
This  is  followed  by  a  summary  of  related  work  on  intelligent  automata. 

Since  much  of  the  research  related  to  the  position  problem  has  occurred 
outside  these  fields,  we  discuss  that  work  in  Chapter  III.  In  the  last 
section  of  this  chapter,  the  contribution  of  this  dissertation  to  current 
research  is  presented. 

1.1  History  of  Remote  Manipulation 

The  development  of  remote  manipulators  followed  closely  the 
development  of  atomic  energy.  As  the  radiation  level  of  atomic  energy 
increased,  so  did  the  hazard  to  the  operator.  Thus,  shielded  environ¬ 
ments  and  equipment  to  handle  the  material  were  needed.  Early 
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experiments  were  carried  out  using  tongs  in  shielded  caves.  For  more 
complex  experiments  it  was  deemed  necessary  to  develop  remote  controlled 
manipulators.  It  was  felt  that  general  purpose  manipulators  could  be 
used  to  replace  much  special  purpose  equipment.  Thus  in  1947,  the 
Argonne  National  Laboratory  began  research  into  remote  manipulators  and 
related  equipment.  The  first  manipulators  built  at  Argonne  had  six 
degrees-of-freedom  controlled  by  mechanical  drives  plus  a  hydraulically 
operated  grip.  Later  versions  were  driven  by  electric  motors.  They 
worked  well  for  simple  tasJcs.  However,  there  was  no  force  feedback, 
making  it  difficult  to  perform  experiments  where  articles  came  into 
contact  with  one  another  [1]. 

In  1948  the  people  at  Argonne  decided  to  develop  manipulators 
having  force  feedback  with  motion  capability  analogous  to  that  of  the 
human  hand.  This  led  to  master-slave  manipulators  in  which  the  motion 
of  the  master  was  mechanically  coupled  to  the  slave  so  that  the  forces 
in  the  slave  would  be  approximately  reflected  in  the  master.  Several 
versions  of  these  were  built  at  Argonne.  One  of  these,  the  Model  8,  has 
been  produced  by  several  companies  and  is  commercially  available  [1,  2, 

3,  4l 

Although  these  mechanically  coupled  manipulators  perform  quite  well, 
they  have  several  drawbacks.  The  main  disadvantage  is  the  mechanical 
connection  which  requires  the  master  and  slave  to  be  physically  close 
together.  This  also  means  that  the  shielding  enclosure  must  be  designed 


*Numbers  in  brackets  designate  references  in  the  Bibliography  (P.  ). 


for  the  linkage.  In  addition  the  strength  of  the  slave  is  limited  by  the 
strength  of  the  operator's  hand.  These  disadvantages  are  offset  in  part 
by  the  fact  that  the  manipulators  are  fairly  inexpensive  and  are  able  to 
perform  intricate  operations  [1,  2,  3,4,  5  J. 

Externally  powered  master-slave  manipulators  using  force  reflecting 
servos  have  been  developed  by  both  Argonne  and  the  General  Electric 
Company.  The  Argonne  machine  is  controlled  with  electromechanical  servo6 
while  General  Electric's  ("Handyman")  is  hydro-mechanica lly  controlled. 
These  manipulators  have  proved  as  effective  as  the  mechanically  connected 
master-slaves.  They  have  the  advantage  that  the  only  connection  between 
master  and  slave  is  an  electrical  cable.  In  addition,  they  have  a 
variable  force  feedback  ratio.  However,  their  use  is  not  as  widespread 
as  the  mechanical  type.  Perhaps  this  is  due  to  their  high  cost  and  the 
complexity  of  the  force  reflecting  servo  system  [1,  6  J 

Powered  manipulators,  not  of  the  master-slave  type  have  also  been 
successfully  developed  by  General  Mills,  Inc.,  Programmed  and  Remote 
Systems  Corporation,  AMF,  General  Electric,  Westinghouse  Electric  Company, 
FMC,  among  others.  They  are  often  used  in  radiation  experiments  along 
with  the  more  precise  mas ter- slaves.  They  are  also  used  in  an  under¬ 
water  environment  on  submarines  [V,  8]  .  Electric  and  hydraulic-powered 
prosthetic  arms  have  also  been  developed  [5,  10]  .  All  these  are  generally 
controlled  by  joy  sticks,  toggle  switches,  or  similar  devices. 

All  of  the  above  mentioned  manipulators  require  the  presence  of  a 
human  operator.  In  their  design  much  effort  is  made  to  have  an  inte¬ 
grated  man-machine  system.  This  is  reflected  in  the  research  of 
Mosher  [b,  11]  ,  Goertz  [12]  ,  and  Bradley  [13]  whose  emphasis  is  directed 


towards  developing  systems  in  which  the  operator  does  not  feel  his 
remoteness  but  is  made  to  feel  as  if  he  wer^  performing  the  task  him¬ 
self.  This  is  accomplished  with  force  reflecting  servo-systems  giving 
kinesthetic  feedback  similar  to  what  a  human  would  feel.  Such  work  will 
have  application  in  materia ls-handling,  underwater  work,  and  perhaps 
earth-moving  equipment.  It  also  may  be  applicable  to  problems  of  remote 
master-slave  manipulators  with  time  delay.  Farrell  [14]  has  indicated 
the  feasibility  of  such  schemes. 

There  are  some  problems  that  the  master-slave  system  doe*  not 
adequately  solve.  Since  the  master-slave  system  by  definition  requires 
a  master,  it  does  not  remove  the  tedium  that  is  basic  to  most  manipulative 
tasks.  In  addition,  for  exploration  of  space,  the  time  delay  will  become 
excessive  for  anything  further  distant  than  the  moon.  Thus  we  have 
motivation  to  develop  manipulator  systems  with  intelligence. 

1.2  Intelligent  Automata 

Computer-manipulator  systems  such  as  AMF's  Versatran  and  Unimation, 
Inc.'s  Unimate  [16]  are  presently  in  use  in  industrial  materials-handling 
situations.  These  machines  are  programmed  to  move  through  a  pre-determined 
series  of  positions.  They  are  used  on  assembly  lines  to  unload  punch 
presses,  conveyor  belts  and  similar  fixed  cycle  type  operations.  Working 
three  shifts  a  day,  they  can  economically  compete  with  human  operators  [15]  . 
However,  they  do  not  have  any  decision  making  ability,  so  that,  if  the 
parts  are  not  in  the  right  position  or  if  the  cycle  time  varies,  these 
machines  will  not  operate  successfully.  In  addition  they  must  be  re¬ 
programmed  for  slight  changes  in  the  process.  It  is  thus  desirable  for 
such  systems  to  incorporate  decision  making  capabilities. 


Ernst  [IS]  ,  using  a  manipulator  equipped  with  sensory  feedback, 
developed  a  hand-computer  system  capable  of  stacking  blocks.  His  system 
was  able  to  learn  about  its  environment  with  information  gained  from 
touch  sensors.  The  work  at  MIT's  Project  MAC  pLS[]  has  recently  extended 
the  work  of  Ernst  to  include  visual  inputs  and  to  develop  a  hand-eye 
system  capable  of  manipulating  objects.  The  aim  of  Project  MAC  is  to 
develop  an  autonomous  system  with  vision  capable  of  performing  manipulative 
tasks  requiring  increasing  levels  of  decision  making  ability. 

Rosen,  Nilsson,  Raphael,  £20,  21,  22]  ,  and  others  at  Stanford 
Research  Institute  have  developed  a  mobile  vehicle  under  computer  control 
that  performs  tasks  in  a  real  environment.  The  primary  goal  is  to  develop 
a  system  receiving  visual  and  other  sensory  information  from  the  vehicle, 
and  then  use  this  information  to  direct  the  vehicle  towards  the  completion 
of  tasks  requiring  the  abilities  to  plan  ahead  and  learn  from  previous 
experience. 

Other  research  in  manipulator-computer  systems  has  been  in  using 
small  digital  computers  to  assist  rather  than  replace  operators  in  manipu¬ 
lative  tasks.  Beckett  [23]  at  Case  Institute,  has  developed  such  a  system 
in  which  a  typical  use  of  the  computer  is  to  find  minimum  transit  time 
paths  and  direct  the  manipulator  around  predefined  obstacles.  In  obstacle 
avoidance  his  routines  keep  the  hand  outside  of  effective  boundaries 
placed  around  obstacles. 

The  Supervisory  Controlled  Manipulator,  is  again  a  system  with 
limited  intelligence  intended  to  assist  rather  than  replace  an  operator. 

For  this  system  Whitney  [24]  developed  a  state-space  model  of  manipulative 
tasks.  He  shows  that  tasks,  such  as  pushing  blocks  on  a  table,  or 


deciding  how  many  and  in  what  order  blocks  should  be  moved  or  pushed 
aside  in  order  to  position  a  new  block,  may  be  expressed  in  terms  of 
discrete  state  spaces.  A  state  is  defined  to  be  the  configuration  of 
the  task  site. 

The  Hand-Eye  Project,  of  the  Stanford  Artificial  Intelligence 
Project  [25]  ,  is  oriented  toward  solution  of  computer  supervised  hand- 
eye  problems  of  increasing  complexity.  Current  work  is  on  basic  problems 
involving  manipulation  of  simple  objects  and  analysis  of  visual  data. 
Eventually  it  is  hoped  that  the  system  will  be  developed  to  the  point 
of  being  able  to  assemble  machines. 

1.3  Contributions  of  this  Dissertation 

In  Chapter  II  the  description  of  manipulators  is  put  on  a  systematic 
basis.  We  present  conditions  leading  to  degeneracy  in  six  degree-of- 
freedom  manipulator  and  conditions  in  which  combinations  of  one  degree- 
of-freedom  Joints  are  kinematically  equivalent  to  more  complex  Joints. 
Finally,  a  catalog  of  existing  manipulators  is  presented. 

The  main  analytical  work  is  presented  in  Chapter  III.  Here  solutions 
to  the  position  problem  are  discussed.  Methods  are  given  to  solve  any 
six  degree-of-freedom  manipulator  containing  three  revolute  Joints, 
whose  axes  intersect  at  a  point,  provided  the  remaining  three  joints 
are  revolutes  or  sliders.  The  extension  of  the  method  to  more  difficult 
arrangements  is  dealt  with  in  the  case  where  only  one  pair  of  revolute 
axes  intersect.  A  method  of  solution  for  a  six  degree-of-freedom 
manipulator  with  three  prismatic  joints  is  also  presented. 

In  Chapter  IV  a  numerical  procedure  based  on  velocity  methods  is 
developed  to  analyze  manipulators  whose  solutions  cannot  be  expressed 


as  in  Chapter  III.  This  procedure,  along  with  the  more  conventional 
Newton-Raphson  method  are  programmed  for  a  digital  computer  and  the 
results  compared. 

In  Chapter  V  methods  are  developed  to  place  the  end  of  a  new  type  of 
digital  manipulator  at  a  specified  position.  A  simple  searching 
algorithm  is  made  more  powerful  by  the  addition  of  look-ahead.  The  three 
dimensional  problem  is  attacked  with  insight  gained  from  studying  a 
planar  model. 

The  trajectory  generation  problem  is  discussed  in  Chapter  VI.  A 
set  of  heuristics  is  given  for  moving  the  manipulator  from  an  initial 
position  to  a  final  position  through  a  space  containing  obstacles. 

Possible  conflict  between  all  links  of  the  manipulator  and  nearby 
obstacles  is  detected,  and  hopefully  avoided. 

In  Chapter  VII  some  considerations  in  choosing  a  manipulator  for  use 
with  a  digital  computer  are  discussed.  The  desirability  of  being  able  to 
arbitrarily  locate  the  hand  throughout  the  workspace  brings  up  the  problem 
of  zones.  Some  insight  into  this  problem  is  presented. 

Much  of  the  above  has  been  programmed  and  tested  on  a  digital 
computer.  In  particular  the  numerical  solutions  and  the  heuristics  for 
trajectory  generation  have  been  programmed  to  result  in  a  fairly  general 
kinematic  package.  With  only  smalli  modification  these  routines  could 
be  used  with  any  six  degree-of-freedom  manipulator. 
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CHAPTER  II 


CLASSIFICATION  OF  MANIPULATORS 


2.1  The  Basic  Model 

In  order  to  analyze  and  compare  manipulator  configurations,  it  is 
desirable  to  develop  a  mathematical  model  that  can  be  used  to  describe 
all  manipulators.  A  manipulator  is  considered  to  be  a  group  of  rigid 
bodies  or  links.  These  links  are  connected  and  powered  in  such  a  way 
that  they  are  forced  to  move  relative  to  one  another  in  order  to  posi¬ 
tion  a  hand  or  other  type  of  terminal  device.  The  first  link  is  assumed 
connected  to  ground  by  the  first  joint  while  the  last  link  is  free  and 
contains  the  hand.  In  addition,  each  link  is  connected  to  at  most  two 
others  so  that  closed  loops  are  not  formed.  For  the  purpose  of  this 
work,  the  assumption  is  made  that  the  connection  between  links  (the 
Joints)  have  only  one  degree-of -freedom.  With  this  restriction,  two 
types  of  joints  are  practical  —  revolute  and  prismatic.*  A  revolute 
Joint  only  permits  rotation  about  an  axis,  while  the  prismatic  Joint 
allows  sliding  along  an  axis  with  no  rotation.  A  schematic  representa¬ 
tion  of  these  Joints  is  shown  in  Fig.  2.1.  If  a  manipulator  is  considered 
to  be  a  combination  of  links  and  Joints,  with  the  first  link  connected 
to  ground  and  the  last  link  containing  the  terminal  device,  it  may 
be  classified  by  the  type  of  joints  and  their  order.  For  example,  a 
manipulator  comprised  of  three  revolute  Joints,  a  prismatic  joint, 

★Although  others  might  wish  to  include  screw  Joints,  we  feel  that  the 
difficulties  encountered  in  building  screw  joints  make  them  impractical. 


and  two  revolute  joints,  in  that  order,  would  be  designated  3R-P-2R, 
where  R  is  used  tor  a  revolute  and  P  for  a  prismatic  joint. 

Given  a  broad  classification  according  to  the  joints,  a  sub-grouping 
is  made  by  looking  at  the  links.  Now,  each  joint  has  an  axis  associated 
with  it,  and  two  adjacent  axes  are  connected  by  a  link.  Thus  a  link 
description  is  just  the  description  of  the  relation  between  two  adjacent 
axes.  A  link  model,  shown  in  Fig.  2.2,  has  the  following  parameters: 

a^:  The  common  normal  between  the  axis  of  the  i—  joint  and  the 

axis  of  the  (i+1)  —  joint. 

s^:  The  distance  between  the  lines  a^  and  ai-l  measured  along 
the  positive  direction  of  the  axis  of  the  i£il  joint. 

0 ^ :  The  rotation  of  the  line  relative  to  the  line  a^-i  about 

the  axis  of  the  i£il  joint. 

0^:  The  angle  between  the  (i+l)£li  axis  and  the  i£h  axis.  The 
positive  sense  is  determined  according  to  the  right-hand 
screw  rule  with  the  screw  taken  along  aj  pointing  from  the 
(i+1)—  to  the  i—  axis. 

If  the  joint  i  is  a  revolute,  then  a^,  s^  and  are  constants 

while  0j_  is  the  variable  associated  with  that  joint.  If  joint  i  is 
a  prismatic  joint,  then  a^,  and  0^  are  constants  while  s^  is 

the  variable.  The  sub-classification  is  then  made  according  to  the 
non-zero  a ^  and  Sj  .  For  example,  if  all  the  a*  and  s^^  of  a 
4R  manipulator  were  non-zero,  it  wouid  have  the  sub-classification 
slals2a2s3a3s4a4  or  ai  =  S£  =  =  0  it  would  be  of  the  type 

sla2a3s4a4‘  may  be  noted  that  for  the  last  link,  i  =  n,  an,an  and 
s  are  not  well  defined  as  axis  n+1  is  non-existent.  For  this  reason, 
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if  fhe  last  Joint  is  a  revolute,  the  parameters  of  the  last  link  will 
not  be  included  in  the  description.  If,  however,  the  last  Joint  is  a 
prismatic  then  sn  will  be  included.  For  the  first  link  has  an 

arbitrary  reference  that  will  be  considered  zero  so  that  s^  will  be 
included  only  if  the  Joint  is  prismatic.  An  example  of  a  4R,  828283 
is  shown  in  Fig.  2.3. 

2.2  Special  Cases:  Degeneracy  and  Kinematic  Eqf1  alence 

The  most  general  manipulator  has  all  non-zero  link  parameters. 
However,  in  practical  manipulators  there  are  many  zero  parameters  which 
lead  to  special  cases  of  interest.  The  first  is  degeneracy.  This 
exists  when  the  number  of  degreea-of- freedom  of  the  last  link  is  less 
than  the  number  of  joints.  A  manipulator  with  more  then  six  Joints 
would  be  classified  in  this  category,  as  a  rigid  body  can  have  a  maximum 
of  six  degrees-of-freedom.  The  existence  of  four  or  more  prismatic 
Joints  leads  to  degeneracy,  since  motion  from  one  Joint  can  in  general 
be  obtained  as  a  linear  combination  of  the  motion  of  the  remaining 
three.  Also,  if  four  or  more  reVolute  axes  always  intersect  at  a 
point,  then  rotation  about  one  axis  can  be  expressed  as  a  combination 
of  rotations  about  the  other  three.  Special  values  of  the  parameter;  a 
can  also  lead  to  degeneracy.  An  example  is  given  by  those  values  of  a 
for  which  four  revolute  axes  are  always  parallel,  and  hence  normal  to 
the  same  plane. 

In  addition  to  degeneracy,  non-zero  parameters  may  make  combina¬ 
tions  of  revolute  and  prismatic  joints  kinematically. equivalent  to 
more  complex  joints.  Thus  if  three  revolute  axes  Intersect  at  a  point 
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axis  ot  rotation  axis  ot  slide 


Figure  2.1.  Schematic  Representation  of  Joints. 

(a)  Revolute  (b)  Prismatic 


Figure  2.2.  The  Link  Model. 


Figu/e  2.3.  Schematic  of  a  4R,  828283  manipulator. 


they  are  equivalent  to  a  spherical  joint  which  we  denote  by  the  symbol 
S.  Also,  if  the  axes  of  a  revolute  and  a  prismatic  joint  coincide,  they 
are  equivalent  to  a  cylindrical  joint  denoted  by  the  symbol  C. 

A  4R  manipulator  may  be  used  to  illustrate  these  special  cases. 

The  most  general  case  is  shown  schematically  in  Fig.  2.4a.  A  sufficient 
condition  for  two  axes  to  intersect  is  that  their  common  normal  be 
zero.  For  example  if  &2  is  zero,  then  axes  2  and  3  intersect 
(Fig.  2.4b).  For  three  axes  to  intersect  at  a  point,  the  two  common 
normals,  as  well  as  the  displacement  along  the  intermediate  axis  must 
be  zero.  For  example,  if  =  a3  *=  s  3  =  0,  the  result  is  equivalent 
to  a  spherical  joint  and  the  4R  manipulator  is  kinematically  equivalent 
to  an  S-R  manipulator  (Fig.  2.4c).  For  four  axes  to  intersect  at  a 
point  (resulting  in  degeneracy),  three  adjacent  conrion  normals,  and  the 
displacements  along  the  two  intermediate  axes  must  be  zero  (Fig.  2.4d). 
Degeneracy  also  occurs  if  the  equivalent  of  two  spherical  joints  exist. 
In  this  case,  it  is  possible  for  the  link  connecting  the  two  sphere 
centers  to  rotate  about  itself. 

A  cylindric  joint  results  when  the  common  normal  and  the  angle 
between  a  revolute  and  adjacent  prismatic  Joint  are  both  zero.  An 
example  of  an  R-P-R  being  equivalent  to  an  R-C  manipulator  is  shown 
in  Fig.  2.5. 

2,3  A  Catalog  of  Manipulators 

With  the  above  scheme  we  may  classify  most  of  the  manipulators  that 
have  been  built  in  the  last  several  years.  Some  manipulators  since 
they  contain  a  very  large  number  of  links  are  omitted  from  the  table. 
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Figure  2.4. 


(a)  A  general  4R, a^s^a^s^a^  manipulator. 

(b)  A  4R,a^S2S2  with  one  pair  of  intersecting  axes. 

(c)  A  4R,a|s2  manipulator  and  spherical  equivalent. 

(d)  A  degenerate  4R  manipulator. 
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These  generally  have  a  snake-like  structure,  and  even  though  these 
manipulators  may  fit  into  the  basic  model  they  contain  many  joints 
usually  with  limited  freedom  in  each  joint  and  similar  link  parameters 
for  all  links.  We  call  such  manipulators  "ORMS"*  and  consider  them 
separately  in  Chapter  5. 

Table  2.1  contains  a  catalog  of  some  recently  built  manipulators. 


*ORM  is  the  Norwegian  word  for  snake. 
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CHAPTER  III 


SOLUTIONS 


3.1  Statement  of  the  Problem 

In  remote  manipulation  it  is  desirable  to  place  a  rigid  body  (the 
hand)  at  a  specified  position  in  space  with  a  specified  orientation. 

Thus,  a  manipulator  needs  to  have  at  least  six  degrees-of- freedom.  More 
joints  than  six  lead  to  a  problem  that  is  not  deterministic  with  the 
specification  of  hand  position  and  orientation.  We  therefore  limit  this 
work  to  manipulators  with  six  degrees-of-freedom. 

The  problem  we  wish  co  solve  may  be  stated  as  follows:  given  the 
desired  hand  position  and  orientation,  along  with  the  various  link 
parameters,  find  the  values  of  the  manipulator  variables  that  place  the 
hand  at  the  desired  position  with  the  desired  orientation.  This  problem 
is  related  to  the  displacement  analysis  problem  in  three  dimensional 
kinematics. 

The  result  of  the  displacement  analysis  of  a  mechanism  is  the 
relationships  between  input  and  output.  That  is,  if  one  link  is  driven 
in  a  prescribed  manner,  we  wish  to  find  the  resulting  position  of  the 
rest  of  the  mechanism. 

The  most  general  one  degree-of- freedom,  single  loop  mechanism  is  the 
so-called  "seven-bar  chain".  This  mechanism  is  composed  of  seven  one 
degree-of-freedom  joints  connected  to  one  another  in  a  general  manner  to 
form  a  single  closed  loop.  Mechanisms  comprised  of  spherical  and 
cyclindric  joints  may  be  derived  from  this  seven  bar  by  an  appropriate 
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choice  of  link  parameters  leading  to  kinematic  equivalence,  as  discussed 
in  Chapter  II. 

If  one  considers  a  seven  bar  mechanism  where  one  link  is  considered 
fixed,  while  an  adjacent  link  is  driven  relative  to  it  by  motion  in  the 
connecting  joint,  then  the  position  and  orientation  of  the  driven  link 
are  known.  The  problem  of  displacement  analysis  is  to  find  the 
resultant  configuration  of  the  mechanism,  or  equivalently  the  motion  in 
each  of  the  remaining  six  joints.  We  then  observe  that  the  manipulator 
problem  resulting  from  specifying  hand  position  and  orientation  is 
analogous  to  the  displacement  analysis  problem  resulting  from  driving  one 
of  the  links. 

3.2  Survey  of  Existing  Solutions 

Although  displacement  analysis  of  mechanisms  has  been  of  interest 
to  kinematicians  for  many  years,  no  method  has  been  developed  that  can 
be  applied  to  all  cases.  Dimentberg  [40,  41 J  obtained  solutions  for 
several  four- link  mechanisms  using  screw  algebra  and  Dual  numbers.  He 
also  reduced  the  five- link  RCRCR  mechanism  to  the  solution  of  a  single 
polynomial  of  degree  eight.  Yang  [42]  using  dual  number  matrices,  was 
able  to  express  the  input-output  relation  of  this  mechanism  as  a  single 
polynomial  of  degree  four.  Others  have  used  (2x2)  dual  matrices,  dual 
quaternians,  and  vector  methods  to  obtain  solutions  of  four  link 
mechanisms  C43,  44,  45J .  The  (4x4)  matrix  method  developed  by  Denavit 
and  Hartenberg  [46J  has  also  been  used  to  analyze  four-link  mechanisms 
[47,  48].  For  more  than  four  links,  this  method  has  been  applied  using 
iterative  numerical  techniques  [49].  Urquardt  [50 J  showed  that  solutions 
were  possible  where  the  mechanisms  had  three  or  more  prismatic  pairs. 
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3.3  Method  of  Solution 


In  this  work,  we  use  (4x4)  matrices  to  attack  the  manipulator 
problem.  Solutions  for  manipulators  containing  three  intersecting 
revolute  axes  are  presented.  The  most  complex  of  these  requires  the 
solution  of  a  single  polynomial  of  degree  four.  This  is  equivalent  to 
the  solutions  of  all  single  loop  five-bar  mechanisms  containing  one 
spherical  joint  and  the  rest  either  revolute  or  prismatic.  Solutions 
for  manipulators  with  any  three  joints  prismatic  are  also  presented.  The 
extension  to  more  difficult  problems  is  discussed  witha  6R,  &2au 
manipulator  having  adjacent  axes  orthogonal  used  as  an  example. 


3.3.1  Notation 

Throughout  the  text  we  use  scalar,  vector,  and  matrix  quantities. 

Matrices  are  denoted  by  capital  letters  and  may  have  subscripts  (e.g.,  A^). 

Vectors  are  denoted  by  underlined  letters  and  may  have  subscripts  and  one 

or  more  superscripts  in  front  of  the  lettei.  Vectors  are  generally  used 

to  locate  points  relative  to  a  coordinate  system.  The  subscripts  are  used 

to  differentiate  between  points,  while  the  superscript  indicates  the  coor- 

i+1 

dinate  system  to  which  the  point  is  referenced  (e.g.,  Xn  ,  would  repre¬ 

sent  a  vector  from  the  origin  of  coordinate  system  i+1  to  a  point  n) . 

If  no  superscript  appears  it  is  assumed  to  be  1  ,  or  else  no  origin  is 
implied.  At  times  we  wish  to  express  a  vector  in  a  coordinate  system  which 
differs  from  the  one  in  which  the  vector  is  formed  (the  so-called  "refer¬ 
ence  system").  If  the  system  used  to  express  these  coordinates  is  different 
from  the  reference  system,  we  enclose  the  vector  in  brackets  and  use  an- 
a1  r,  '0'»’-script  to  denote  the  system  in  which  the  components  are  expressed 


^.g.  , 


i+1 


). 


Tf  the  outer  superscript  is  not  used,  it  is  assumed 
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to  be  the  same  as  the  inner  superscript.  Scalar  quantities  are  written 
as  lower  case  letters,  with  or  without  subscripts  (e.g.,  a^s^  )•  If 
they  represent  coordinates  of  points,  then  a  superscript  is  sometimes 
used  to  designate  the  coordinate  system  to  which  they  refer.  Where  no 
superscript  is  used,  the  number  1  is  implied.  Angles  are  denoted  by 
lover  case  Greek  letters  with  or  without  subscripts  (e.g.,  &  )  . 

Points  are  occasionally  given  a  name  (e.g.,  "the  point  X2  ")  and 
referred  to  by  name. 

The  trigonometric  functions  sin,  cos,  and  tan  are  abbreviated 
s,  c,  and  t  respectively  (e.g.,  sin  H.  is  written  sB^  ,  cosOL  ^ 
as  cc^  ,  etc). 


3.3.2  Mathematical  Preliminaries 

In  order  to  analyze  the  kinematics  of  a  manipulator,  we  first 
establish  the  relation  between  two  Cartesian  coordinate  systems  as 
shown  in  Figure  3.2.  We  define  the  following: 

a^:  the  length  of  the  common  normal  between  iz-axis 

and  *+^z-axis  . 

the  angle  between  *+^z  and  *z  measured  in  the 

right-handed  sense  from  i z  along  a  line  from  *z 
i+1 


to  z  . 

s^:  distance  from  0^  to  the  common  normal  a^  . 
angle  the  common  normal  makes  with  *x-axis. 

Then  there  exists  the  transformation  [46]  to  express  the  coordinates  of 
a  point  in  one  system  given  its  coordinates  in  the  other.  If  we  denote 
the  coordinates  in  system  i  by  (*x,  *y,  *z)  and  in  system  i+1  by 
(i+1x,  i+*y»  i+*z)  >  we  define  the  vectors  *X  and  i+*X  such  that: 
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and 


i+1 

x 

1+iy 

i+1 

z 

1 


so  that  the  transformation  is: 


i 

X  =  Ai 


i+1x 


where 


c«i 

-*Vai 

s  Ojsai 

aic  0. 

s 

c«lcai 

-cS^sC^ 

aisSj 

0 

sa. 

ca. 

s , 

i 

i 

i 

0 

0 

0 

1 

The  inverse  also  exists  and  is  defined  by: 


i+1. 


Ai"1  *X 


where 


c®i 

8  0. 
i. 

0 

'ai 

•s9icai 

cVai 

^i 

■SiCtti 

s  8^sa^ 

-c6i8ai 

o 

p 

H* 

*8icai 

0 

0 

0 

1 

(3.1) 


(3.2) 
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For  n+1  coordinate  systems  there  are  n  transformations  between 
neighboring  systems.  These  may  be  multiplied,  in  the  following  order, 
to  give  the  coordinates  in  the  1  system  of  any  point  fixed  in  the 
n+1  system: 

h  -  AiA0  ...  A  n+1X 

Now  to  appropriately  fix  these  coordinate  systems  in  a  manipulator,  we 
make  *z  correspond  to  axis  i  ,  *x  to  common  normal  a^.^  and 
define  *y  in  a  right-handed  sense.  This  is  shown  applied  to  a  sample 
manipulator  in  Figure  3.3.  For  a  six  degree-of-freedom  manipulator  we 
write: 

h  =  a1a2a3a4a5a6  7X  (3.3) 

where  *X  is  a  vector  to  any  point,  expressed  in  the  ground  system 
and  7X  is  a  vector  to  the  same  point  expressed  in  a  system  fixed  in 
the  terminal  device.  We  define 

Aeq  =  A1  . . .  Afi  .  (3.4) 

With  this  definition  (3.3)  becomes: 

lX  =  Aeq  7X  (3.5) 

and  the  inverse  yields: 

7X  =  Aeq'1  :X  (3.6) 

Now,  if  we  let  P  be  a  vector  from  the  origin  of  system  1  to  the 
origin  of  system  7,  and  -Z  ,  m  ,  and  n  ,  be  three  unit  vectors 
aligned  with  the  7x,  7y,  7z  axes  respectively,  then  when  ,  m  ,  n  , 
and  P  are  expressed  in  system  1,  they  may  be  used  with  equation 
(3.5)  to  find  Aeq  .  That  is,  using  (3.5)  we  may  write 
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It  is  thus  seen  that  position  and  orientation  of  the  terminal  device 
can  easily  be  found,  knowing  the  manipulator  variables,  0^  or 
s^,  i— 1 , . . ,  6  ,  by  the  matrix  product  equation  (3.4). 

However,  for  computer  control  of  manipulators,  the  problem  is  to 
find  the  manipulator  variables,  given  the  terminal  position  and 
orientation  (Aeq)  . 

We  shall  first  consider  a  six-revolute  arm  and  the  problem  of 
finding  0  ,  ...,  9^  given  Afeq.  Equation  (3.4)  represents  twelve 
scalar  equations,  nine  dealing  with  orientation  and  three  with  position. 
However,  only  three  of  the  orientation  equations  are  independent  so  that 


there  are 

six  equations 

in  9  , . . . 
i  ’ 

•V 

These  equations  have  terms  of 

the  form: 

c6l  c&2  c63 

c04 

c95 

c06  , 

(3.8) 

s«l  c02  c<^3 

sH4 

sSj 

se6  , 

These  terms  contain  both  sines  and  cosines,  which  we  may  define  in  terms 
of  the  tangent  of  the  half-angle. 


Then  if  we  substitute  (3.9)  into  the  six  equations,  the  typical  term, 
as  shown  in  (3.8)  becomes  (letting  t^  =  tan  -^-i  ,  1=1,...,  6  ,  and 
removing  the  denominators  which  are  common): 

2  2  2  2  2  2 
fcl  t2  fc3  C4  C5  t6  +  “• 

Thus  we  see  that  these  equations  are  quadratic  in  each  of  the  unknowns 
and  the  degree  of  the  highest  degree  term  is  12. 

However,  not  all  the  equations  contain  all  of  the  unknowns  and  by 
judiciously  choosing  the  three  orientation  equations,  the  unknowns  8^ 
and  8^  can  be  eliminated  from  some  of  the  equations.  We  use  the 


six  equations: 

Fj  (tj,...,  t5)  «  0  (3.10) 

F2  (t1 .  t5)  *  0  (3.11) 

F-j  (t^,...,  t5)  -  0  (3. 12,) 

F4  (t2,...,  t5)  *  0  (3.13) 

F5  ^2 *  •  *  *  *  ^5)  =  ®  (3.14) 

F^  *  *  *  *  *  *-6^  =  ®  (3. 15) 


which  are  obtained  respectively  from  the  '14',  '24',  '13',  '33',  '34', 
and  '32',  elements  of  the  matrix  of  (3.4).  We  note  that  (3.10)  -  (3.14) 
do  not  contain  tg  ,  and  (3.13)  -  (3.15)  do  not  contain  t^  .  Of  the 

five  equations  in  which  the  variables  t^,...,  t^  appear  at  most 
quadratically,  three  equations  are  of  degree  10,  while,  two  are  of 
degree  eight.  If  we  eliminate  t^  between  (3.10),  (3.11),  and  (3.12), 
the  result  is  two  equations  of  at  most  degree  eight  in  the  unknowns 
t2»...,  whose  total  degree  is  32.  These  together  with  (3.14)  and 
(3.15)  give  us  four  equations  for  .  Proceeding  in  this 
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manner  eliminating  one  variable  at  a  time,  we  would  finally  obtain  a 
single  polynomial  of  degree  524,288.  Even  though  this  method  of 
elimination  introduces  extraneous  roots,  we  would  still  expect,  according 
to  Bezouts1  theorem*,  (10)  x  (8)  or  64,000  common  roots,  a  number  much 
too  large  to  cope  with.  The  general  problem,  attacked  in  this  manner, 
is  insoluble.  At  this  point  we  shall  define  a  "soluble  case"  to  be  one 
in  which  the  degree  of  the  final  eliminant  is  low  enough  to  find  all 
roots.  In  practice  all  the  roots  of  an  eighth  degree  polynomial  can  be 
found  within  a  few  seconds  using  a  digital  computer  and  the  roots  of  a 
fourth  degree  within  one-half  second.  A  solution  is  said  to  be 
"c losed- f orm"  if  the  unknowns  can  be  solved  for  symbolically. 

Even  though  the  general  problem  is  beyond  reach,  many  practical 
manipulator  configurations  are  soluble.  The  existence  of  three  revolute 
axes  intersecting  at  a  point  leads  to  a  soluble  class.  In  the  next 
sections  we  explore  the  possible  combinations  of  three  intersecting  axes. 


3.3.3.  Last  Three  Axes  Intersecting 

If  the  last  three  joints  are  revolutes  and  their  exes  intersect 
as  in  Figure  3.4,  then  their  point  of  intersection,  as  designated  by  the 
vector  P3  is  only  a  function  of  motion  in  the  first  three  joints  and 
the  constant  link  parameters.  is  known  by  specifying  the  hand 

position  and  orientation.  We  want  to  solve  the  three  scalar  equations 
represented  by: 


^3 


=  A 


1A2A3 


0 

0 

I4 


(3.16) 


*Bezouts'  theorem  gives  an  upper  bound  to  the  number  of  common  solutions 
for  a  set  of  equations.  The  upper  bound  is  the  product  of  the  total 
degrees  of  all  the  equations. 
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for  the  variables  associated  with  the  first  three  joints.  We  now  derive 
an  important  result  used  in  the  solution  of  this  problem.  We  define 


the  vector  Pj 


Pj  "  Aj 


‘J 


0 

0 

sj+l 

1 


where  Aj  (i  *  1 .  j)  is  defined  in  equation  (3.1).  It  is 

that  Pj  is  a  vector  specifying  the  position  of  a  point  (0,  ( 
which  is  fixed  in  coordinate  system  j+1  . 

We  may  write  (3.17)  as 


_Pj  *  (AjA2) 


J+1 


A1A2 


f  1  (  e3 » 
f  2 ( ®3» 


*J> 


f2(S3 .  V 


where 


fl 

0 

f  2 

0 

f3 

■ 

> 

U) 

> 

u. 

8  J+1 

1 

1 

Then  using  (3.1)  for  Aj  and  A£  (3.18)  becomes 

C0181  +  80182 
seigl  -  ct)xg2 

^sO,j[s02(a2  +  fj)  “  c02("ca'2^2  ®^2^3^ 

+  cCt ^ (sO^f  2  +  c&2f  ^  8 2^  +  8 1 


(3.17) 

seen 
’  8j+l) 


(3.18) 


(3.19) 


(3.20) 
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where 


gx  =  cB^a^fj)  +  s02(-ca2f2  +  sa2f3)  +  3l  (3.21) 

g2  =  ”S02cOC1(a2+f1)  +  c02ca1(-ca2£2  +  sd.2f3) 

+  sa1(sa2f2  +  ca2f3  +  s2)  .  (3.22) 

Denoting  the  components  of  Pj  by  Xj  ,  yj  ,  Zj  ,  v  define 

Rj  =  x5  +  yj  +  <Zj*s])2  (3.23) 

With  (3.20)  for  the  components  of  fPj)  ,  (3.23)  becomes 


Rj  =  fl2  +  f22  +  f32  +  al2  +  a22  +  s22  +  2a2f 1 
+  2s2(sa2f2  +  ca2f3)  +  2aiL  cYa2+f  1^ 

+  sP2(-ca2f2  +  sa2f3)]  (3.24) 

We  note  from  (3.20)  and  (3-24),  that  we  may  write: 

Rj  =  (FjC p2  +  F2sp2)2ai  +  F3  (3.25) 

Zj  =  (FlS02  -  F2c82)sa1  +  F4  (3.26) 

where, 

F1  "  a2  +  fl  (3-27) 

F2  =-ctt2f2  +  sa2f3  (3.28) 

F3  =  ^l2  +  F22  +  ^ 3 2  +  ai2  +  s22  +  2a2Fi  +  ®2 

+  2s2('s<12f2  +  ca2f3)  (3.29) 

F4  =  ca1(sa2f2  +  ca2f3+s,)  (3.30) 


Equations  (3.25)  and  (3  26)  prove  to  be  very  useful  as  8^  has 
been  eliminated,  and  82  appears  in  a  very  simple  form. 

Returning  to  the  manipulator  problem,  the  above  equations 
apply  with  j  =  3.  In  which  case  by  using  (3.1)  for  A-  (3.19) 
becomes 


32- 


fl 

-  — 
8489^803+8^093 

f2 

= 

-84093803+83803 

f3 

84003+83 

(3.31) 


so  that  with  (3.21),  (3.22),  and  (3.31)  equation  (3.20)  represents 
three  equations  in  three  unknowns.  If  the  first  three  joints 
are  prismatic,  then  (3.20)  represents  three  linear  equations  and 
is  easily  solved.  The  othe^  possibilities  are  somewhat  more 
difficult,  but  may  be  solved  as  follows: 


3  Revolute  -  9] ,  9? ,  all  variable 

Substituting  (3.31)  in  (3.27)  -  (3.30)  yields  respectively 

Fi  =  32+84803803+83003 

F2  =-ca,2  (-8^938^3+83803)  +  8a2(83+s^ca3) 

F, 


2  2  2  2  2  2 

fll  +82  +a2  +83  +83  +84  +2s283ca2+2828^ca2ca3 


+  2s3  84ca.3+c93  (2a  283-2828^80.2  8(13)  +  893(2a3S2sa2 

+  2a2s4sa3) 

F^  =  Co,  La3s93S02+83ca2+82+84^"ce38a2sa3+ca2ca3^ 

Now  we  note  that  the  left  hand  side  of  (3.25)  and  (3.26)  are 
known  and  that  if  =  0  ,  (3.25)  reduces  to 

r3  ■=  f3 

When  (3.34)  is  used  in  (3.36)  it  is  simply  a  function  of  93 
Then  making  the  additional  substitution 

1  — tan^  93 

c90  =  - Z_ 


1+  tan  ) 
2 


(3.32) 

(3.33) 


(3.34) 

(3.35) 


(3.36) 


(3.37) 
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2  tan 

s0,  =  1  (3.38) 

J  1  +  tan  83 

2 


into  (3.35),  yields  a  quadratic  in  tan  0^  .  Similar  simplifi- 

7 

cation  results  if  sa^=0  ,  as  (3.26)  reduces  to  a  quadratic.  If 
however  sc.^  and  a^  are  non- zero,  we  eliminate  s02  and  c©2 
from  (3.25)  and  (3.26)  to  obtain  the  polynomial 


(*3  ~  F3)  (  <2- 


2a  i 


so. 


2  2 
=  fl2  +  f2 


(3.39) 


Upon  making  the  tan  03  substitution  and  using  (3.27) 

1 


(3.30)  equation 
After  getting  ©3  ,  02 


(3.39)  is  of  degree  four  in  tan  ©3 

1 

may  be  obtained  from  (3.25)  or  (3.26)  and  0j  from  (3.20) 


Si  »_§2jl_§3  variable 

Here  we  take  the  x  and  y  components  of  ^3  as  defined 
in  (3.20) 


x  =  cO^!  +  s0lg2 
y  =  s01g1  -  c0jg2 

Solving  for  gi  and  g2  we  find 
g!  =  xc,©!  +  ysOj^ 
g2  =  -yc0i  +  xs@1 

so  that  gi  and  g?  can  be  computed  from  v3.42)  and  (3.43). 

Then  examining  (3.21)  and  (3.22)  using  (3.31)  we  note 


(3.40) 

(3.41) 


(3.42) 

(3.43) 


gx  =  c02hi (0^)  +  s02h2(03)  +  ax  (3.44) 

g2  =  ca![c02h2(03)  -  s02h! (03)  ]  +  sa.^^-j)  (3.45) 
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where 


h1  =  s4s03sa3+a3C03 

hz  -  s^(c03cxi2srr3+stt2sa3^  *  a3s03c^2+s3sa2 
h3  =  s4(‘c63sci.2soc3+ca2cnL3^  +  a3se3s°L2 


+  S3CCI2  +  s2 

If  cai  =  0  then  (3.45)  is  easily  solved  for  ©3 
ca.j  ^  0  we  eliminate  @2  from  (3.44)  and  0.45) 
polynomial  2 


+  Ylr 


-  <*rai: 


g2-saih3 


.  If 

to  get  the 


Expressing  s0-j  and  c0^  in  terms  of  tan  ©3  leads  to  a 

J  7 

polynomial  of  degree  four.  Upon  obtaining  the  four  roots  of 
(3.49)  we  substitute  into  (3.44)  and  (3.45)  to  get  ©2  and 
finally  (3.20)  for  . 


(3.46) 

(3.47) 


(3.48) 


(3.49) 


®Li__®2>— §3  variable 

Solve  (3.26)  for  82  >  using  this  in  (3.25)  results  in  a 
fourth  degree  polynomial  in  tan  ©3  .  Then  proceed  as  in  all 
revolute  case. 


— Lt—^2*— 3  -  variable 

Similar  to  0^02@3  variable  with  the  exception  of  s^ 
being  the  variable  in  the  final  polynomial  which  is  of  degree 
four. 


s 13203  variable 

The  left-hand  side  of  (3.44)  may  be  computed  from  (3.42),  then 
(3.44)  which  is  quadratic  may  be  solved  for  0^  .  Finally  S3  and  S2 
may  be  found  from  (3.20). 
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1 


2l§2®3  variable 

It  is  possible  to  eliminate  0£  as  in  the  case  of 
variable,  resulting  in  a  quadratic  in  s^  . 

■§11223  variable 

Equation  (3.25)  is  solved  for  S£  and  used  in  (3.26)  resulting 
in  a  quadratic  in  S3  ,  0^  is  found  as  in  the  all  revolute  case. 

Methods  have  been  presented  to  find  the  first  three  variables. 

At  this  time  we  leave  the  problem  of  finding  the  lest  three  angles  to 
be  dealt  with  later  in  this  work  (see  Section  3.3.6). 


3.3.4  First  Three  Axes  Intersecting 

Next  consider  the  three  intersecting  axes  to  be  the  first 
three,  as  in  Figure  3.5.  The  solution  of  these  is  analogous  to 
the  previous  exarrrle.  We  define  a  vector  7P  fr0m  the  hand  to 
the  point  of  intersection  of  the  three  axes,  as  shown  in  Figure  3.5. 
We  note  that  when  7P  is  expressed  in  a  coordinate  system  fixed 
in  the  hand,  that  it  is  just  a  function  of  the  last  three  joints. 
That  is:  -i 


(3.50) 


Using  (3.2)  for  A3 


fo- 

and  forming  A3"*  0 

L*j 


we  get 


7P 


If  we  use  (3.2)  to  express  A^ 
right-hand  side  of. (3.51)  just 


-1 


-a3 

-S3SCL3 

-830(13 


(3.51) 


,  A5  *  ,  and  A4  *  then  the 


contains  the  three  variables  associated 
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with  the  last  three  joints.  In  addition,  we  compute  the  components 


of  7P  from 


7f>  =  Aeq  _1 


0 

1 


where  Aeq  is  the  known  matrix  (3.7).  We  note  that  the  rotation 
portion  of  Aeq  *  is  just  the  transpose  of  the  rotation  portion 
of  Aeq  .  In  fact,  if 


Aeq  = 


then 


Aeq 


-1  _ 


all 

a  12 

a  13 

ai4 

a21 

a22 

a23 

3  24 

a31 

a32 

a33 

a34 

0 

0 

0 

1 

all 

a21 

a31 

•u1' 

a12 

a22 

a32 

a24~  1 

a13 

a23 

a33 

a34_  1 

0 

0 

0 

1 

(3.52) 


(3.53) 


The  elements  denoted  as  a^’^  ,  ^  »  a34  ^  are  determined  by 


simply  applying 


thus 


Aeq” *  Aeq  =  I 

ai-4  =  "(aliai4  +  a2ia24  +  a3ia34) 
i=l,2,3 


(3.54) 


From  this  point  on  the  method  of  solution  follows  the  same  steps  given 
in  Section  3.3.3  for  the  case  of  the  last  three  axes  intersecting. 
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3.3.5  Three  Intermediate  Axes  Intersecting 


Another  possibility  is  for  the  three  intersecting  axes  to  br 
located  as  in  Figure  3.6,  where  there  are  two  other  joints  toward 
the  base  end  and  one  on  the  hand  end.  We  denote  the  position  of 
the  point  of  intersection  by  X2  with  the  coordinate  (x7,  y^,  z2)  , 
and  define  the  vector  X2  from  the  base  of  the  arm  to  X2  and  the 
vector  7Y2  from  the  origin  of  system  7  to  X2  as  in  Figure  3.6. 

Consider  the  case  where  all  joints  are  revolutes,  then  in 
system  7,  the  hand  system,  the  point  X2  has  a  fixed  z  co¬ 
ordinate,  and  is  a  constant  radius  from  the  origin.  We  write 
the  coordinates  of  X2  in  system  7,  using  equation  (3.5)  and 
Aeq  as  defined  in  (3.53) 

7x2  =  aux2  +  a21y2  +  83^  +  a^  1  (3.55) 

1 ■  ai2y2  +  *22^2  +  a32z2  +  a24_1  (3.56) 

?z2  =  a13y2  +  a23y2  +  a33z2  +  a34  1  (3-57) 

Since  7z2  is  a  constant,  say  ,  (3.57)  may  be  written 

:1  =  al3x2  +  a23y2  +  a33z2  +  a34  (3.58) 

We  define  the  constant,  C2,  to  be  the  square  of  the  radius 

c2  =  (?x2)2  +  (7y2)2  +  (?z2)2  (3.59) 

Then  using  (3.55),  (3.56),  and  (3.57)  for  7x,  7y,  and  7z 
(3.59)  becomes 

C2  =  x2  +  y\  +  z2  '  2x2a 14  '  2y2a24  "  2z2a34 

+  a  ?4+  a24  +  a24  (3.60) 

where  (3.54)  has  been  U3ed  for  a^  *  i  =  1,2,3 


-39- 


n 

fi 

n 


Figure  3.6.  Manipulator  with  three  intermediate  revolute  axes  inter¬ 
secting  (  i.e.  a^=s^=a^=0  ), 
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With  j  =  2  (3.20  becomes 


—2 


[a2(c91c02-s9is02ca1)  +  s2seisa1+a1c01  +  s3(c91s92sa2  + 
s9^c02ca^sa2+s9^sa^ca2)  ] 

[a2(s0^c02+c9is02ca^)  -  s2c0^sa.3  +  a^s9^ 

+  S3(s01s92sa2-c91c92caisa2“c91sa1ca2)] 

[a2s92sa1  +  s2ca1  +  sj  +  s3  (-c92sa1sa2+caica2)  ] 

1 


and  (3.27)  -  (3.30)  become 

F1  =  a2 
f2  =  s3sa2 

F3  =  a^+s2+a^+s3+2s2s3ca2 
F4  ~  s3ca1ca.2+s2 


(3.61) 


(3.62) 

(3.63) 

(3.64) 

(3.65) 


So  that  using  (3.62)  -  (3.65)  and  (3.23),  equation  (3.39)  becomes 

|2 

2  2  2 
“  a2+s3sa2 


.2  2  2  2  2  2  2  .  . 
(x2+y2+z2’  ®  ^"®2"^2"^3"^2^3^2 

2 

+ 

z2*s3c^1c^2"s2 

2ai 

“l 

(3.66) 


Then  (3.58),  (3.60)  and  (3.66)  are  three  equations  for  the  unknowns 
(X2Y2Z2).  Ordinarily  the  system  would  result  in  an  eighth  degree  eliminant 
but  since  (3.60)  and  (8.66)  may  be  combined  to  form 

(C2+2’12a14+2’,2a24+  ’•22a34-a14-a24-a34)-al-s2-a2-s3’28283m2 ' 2 


2a 


1 


z2-s3ca1ca2-s2 


sa. 


2  2  2 
=  a2  +  s3sa2 


(3.67) 
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The  equations  (3.58)  (3.60)  and  (3.67)  may  be  combined  to  yield 
a  single  fourth  degree  polynomial  in  one  variable,  say  z  . 
After  the  values  of  z  are  determined  it  is  possible  to  back 
substitute  and  obtain  corresponding  values  for  x  and  y  . 

Once  the  coordinates  (X2,  y£,  of  the  point  X£  are 

found,  and  0^  may  readily  be  found  from  equation  (3.61). 

0g  is  easily  evolved  by  noting: 


—  “ 

“ “  — 

7 

*2 

“a5 

7?2 

7„  .  "I 

"858a5 

7'2 

—2  A6 

-85°“5 

1 

1 

Using  (3.2)  for  Ag  *  ,  with  a^  «  Sg  *  0  ,  (3.68)  becomes; 


-a5c06  -  s5s06 

a5s06ca6  +  85(-c06sa5ca6-ca5sa6) 
-a^s06sa6  +  85(c0gsa5sa6-ca5ca6) 
l 


(3.69) 


Since  x2,  y2  and  Z£  are  known  (3. 55), (3.56)  and  (3.57)  may  be 
used  to  calculate  7  Y2  •  Then  (3.69)  may  be  solved  for  0g  .  The 
problem  of  solving  for  0^  0^  0^  will  again  be  deferred  (see 
Section  3.3.6). 

The  preceding  solution  was  for  all  revolute  Joints.  We  now 
consider  the  cases  in  which  joints  1,  2,  and  6  may  be  prismatic. 


s^0 2§6  variable 

Eliminating  s  0£  and  c©2  between  the  x-  and  y-  components 
of  (3.61)  results  in  a  quadratic  in  X2  and  y£  .  Then  this  equation  along 
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with  (3*58)  and  (3.60)  can  be  reduced  to  a  single  fourth  degree  poly 
nomial  in  either  x^  or  y£  • 

—6  variable 

Forming  (3.25)  and  (3.26)  with  j  =  2  aud  then  eliminating  S£ 
between  them,  a  fourth  degree  equation  results  in  a  manner  similar 
to  the  all  revolute  case. 


ll®2§6  variable 

First  &2  nay  be  eliminated  between  the  x-  and  y-  components 
of  (3.61).  The  resultant  is  a  linear  equation  which  along  with  (3.58) 
and  (3.60)  can  be  combined  to  form  a  single  quadratic. 

If  s6  is  variable  instead  of  0^  ,  equations  (3.58)  and  (3.60) 
no  longer  apply.  However,  the  point  X2  must  lie  on  a  known  line. 
This  line,  in  the  direction  of  axis  5  may  easily  be  found,  and  may  be 
written  in  terms  of  two  known  constant  vectors  £  and  l)  and  the 
parameter  t  as: 


x2 

*2 

z2 


=  c  +  b  t  , 


(3.70) 


where  b  is  a  unit  vector  parallel  to  this  line  and  c  is  any  fixed 
point  on  the  line.  Eliminating  t  ,  yields  two  equations  between 
x2  *  3^2  »  an^  z2  •  Then  with  these  in  place  of  (3.58)  and  (3.60), 
the  procedure  is  the  same  as  previously  indicated. 

The  second  possibility  for  three  intermediate  axes  to  intersect 
is  as  shown  in  Figure  3.7.  This  is  just  an  inversion  of  the  case 
treated  in  this  section  and  may  be  solved  in  a  similar  manner. 
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3.3.6  Completing  the  Solution 

It  can  be  seen  from  the  foregoing  that  if  three  adjacent  revolute 
axes  intersect  at  a  point,  then  the  solution  to  the  problem  can  be 
reduced  to  a  single  equation  of  degree  four.  If,  in  addition,  two  of 
the  remaining  three  joints  are  prismatic,  the  problem  reduces  to  a 
quadratic. 

Simplification  will  also  result,  if  special  geometry  exist  in 
addition  to  the  three  intersecting  revolute  axe3.  Consider  the  all 
revolute  case,  with  only  a^  ,  a2  ,  and  s^  non-zero  and 
04  =  90°,  0.2  =  0  »  a3  =  90°,  a4  =  90°,  <3.5  =  90°,  as  shown  in 
Figure  3.8.  This  is  the  configuration  used  for  the  hydraulic  arm  at 
the  Stanford  Artificial  Intelligence  Project.  With  the  above  values, 
equation  (3.17)  becomes 

[a"jC0j  +  a2C0jC02  +  s^(c0  ^c02s03+c0  }S02e03) 

IV0i  +  a2s0lC02  +  SA(S01C02S03+S01S02C03)  j  (3.71) 

a2s02  +  S4(s02s03-c02c03) 

—  1 

and  (3.27),  (3.28),  (3.29),  and  (3.30)  become 


—3  " 


Fi  =  82+84303 
E2  =  S4C0.J 

F3  =  2a2s^s03  +  84^  +  a^-  + 

F4  =  0 

So  that  equation  (3.25)  becomes: 

2  j  2 

R3  =  S4  +  a2  +  a  1  +  2a2S4S©3  +  2a^a2c©2  +  2a^s^(c02S0^ 

+S02C0-j) 


(3.72) 

(3.73) 

(3.74) 

(3.75) 


(3.76) 
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Figure  3.7.  Second  possibility  for  the  case  of  the  three  intermediate 
revolute  axes,  shown  intersecting  at  the  point  x^. 


matic  of  the  6R,  a  as  manipulator  used  at  the 
ford  Artificial  Intelligence  Project,  with  o<  =90°, 
,  <*3=?0°,  °<4=90O,  0^=90°.  1 


hei 

an 

=0 
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I 


and  (3.39)  becomes: 

_  __  2 

2  2  2 

R^- (2a,s, s0<,+s,.  -fa,  +ai  )  ?  2  2  2 

- . ^-4—3—4 - 2 - L_1  +  z2  =  (a2+s4s03)^  +  s4  c032  (3.77) 

_  231  _ 

which  i3  quadratic  in  s©3  when  c©3^  is  replaced  by  1-80^^  . 

After  finding  03  we  compute  ©2  from  (3.71)  and  (3.76)  and  0^ 
from  (3.71). 

Since  the  above  arm  is  used  in  the  Stanford  Artificial  Intelligence 
Project,  we  shall  use  it  to  illustrate  the  method  of  finding  the  angles 
associated  with  the  three  intersecting  axes.  Designating  the  direction 
of  the  iiii  axis  by  the  unit  vector  ,  we  write 


m  ■  ^2*^4 


(3.78) 


Using  (3.1)  for  A^,...,  A4  and  the  above  values  of  a  the 


result  is 


c0jc02S03  +  c0is@2C03 
80JC02S03  +  s0js02C@3 


s©2S03  -  c©2C03 


(3.79) 


so  that  U)4  may  be  computed  from  (3.79)  as  we  have  solved  for  0^  , 
©2  »  and  03  .  ^  is  known  since  the  hand  orientation  is  specified. 


In  addition. 


m  •  ia5  ■  cosrt4 
45  •  46  “  cosa5 


(3.80) 

(3.81) 


45  •  45  "  1 


(3.82) 


where  n.^  and  are  link  parameters  of  the  arm.  In  fact 

e  -90°  and  a5  *  90°  .  We  can  find  the  components  of  oj5  by 
simultaneously  solving  (3.80),  (3.81)  and  (3.82).  We  observe 


7Qj!5]  "  V^s"1 


0 

0 

1 

0 


(3.83) 


using  (3.53)  for  1  and  1  with  -  90°  and  a&  -  0 


(3.83)  becomes: 


[Wc] 


and 


s0( 

c6f 

0 

_  0. 

-1 


(3.84) 


(3.85) 


7Cii5]  *  Aeq  11*5 
where  Aeq  is  the  known  matrix  specifying  hand  position  and 
orientation  equation  (3.7).  Its  inverse  is  found  as  in  Section  3.3.4. 
So  that  we  easily  derive  0^  by  equating  the  right-hand  sides  (3.84) 
and  (3.85).  We  also  write 


Cm! 


A  A.  A 
6  5  4 


0 

S0,jC0g 

1 

0 

-sS^sQg 

1 

-cQj 

0 

0 

(3.86) 


and 


7Cm]  * 


(3.87) 
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To  obtain  0. 

4 

we 

proceed  similarly 

0 

sO.cO.cO,  -  C0, 80,  j 
4  5  6  4  6 

0 

-S0.C0  80,  -  C0.C0, 

XA  -1A  ~ 1 

= 

4  5  6  4  6 

4  3 

1 

80^S0tj 

0 

1 

O 

J 

(3.88) 


and 


7[u)3]  *  Aeq"1  uj 


(3.89) 


which  yields  0^. 

We  have  indicated  a  procedure  to  find  the  rotation  about  three  inter¬ 
secting  revolute  axes  when  these  are  located  at  the  hand.  The  method 
is  applicable  when  any  three  axes  Intersect.  However,  the  equations 
must  then  be  rewritten  in  terms  of  the  ^  and  0^  associated  with 
these  axes. 


3.3.7  Solution  for  Any  Three  Joints  Prismatic 

A  six  degree- of- freedom  manipulator  with  any  combination  of  three 
revolute  and  three  prismatic  joints  is  soluble.  This  arises  from  the 
fact  that,  the  orientation  of  the  hand  is  independent  of  the  displacement 
in  the  prismatic  joints,  and  is  only  a  function  of  rotation  in  the  three 
revjlute  joints.  In  addition  the  orientation  is  independent  of  the 
position  in  space  of  the  revolute  axes.  Consider  the  manipulator  shown 
schematically  in  Figure  3.9.  The  direction  of  the  first  revolute  axis 
is  always  fixed.  With  the  hand  orientation  specified,  the  direction  of 
the  third  revolute  axis  becomes  fixed.  In  addition  we  know  the  angles 
which  the  axis  of  the  second  makes  with  the  axes  of  the  first  and  third 
revolutes.  If  we  designate  the  direction  of  these  revolute  axes  by  the 


unit  vectors,  ^  »  $3  »  and  W5  c^en  we  My  write 


mi 

.  ujj  *  cosBj^ 

(3.90) 

m 

.  ■  CO8B2 

(3.91) 

J«3 

•  %  “  1 

(3.92) 

where  B^  and  0^  are  the  known  angles.  The  equations  (3.90) 

(3.91),  (3.92)  are  then  solved  £or  the  components  of  .  The 
joint  angles  may  be  found  in  a  manner  analogous  to  that  used  in 
the  previous  example,  as  the  now  known  direction  ^  ,  can  be 
expressed  only  as  a  function  of  62  which  leads  to  a  simple 
equation  for  6^  •  IU3  can  also  be  written  in  terms  of  0^  alone, 
yielding  0^  .  Once  @2  and  0^  are  known,  0^  is  easily  found 
by  rewriting  (3.4)  as 

A  A  -1.  'lA  A-1A  -1A  -1 

Aj  ■  A2  Aj  Aeq  A j  A^.  A^ 

Using  the  values  we  fouud  for  0^  and  0^  plus  the  constant  angles, 
we  compute  the  rotation  portion  of  the  right-hand  side  of  the  above 
equation.  Then  writing  as  in  (3.1),  we  may  solve  for  cO^  and 

s0^  ,  thus  finding  0^  .  The  displacements  in  the  prismatic  joints  may 
be  found  from  (3.4).  Since  all  the  angles  are  now  known  and  the  s's 
only  appear  linearly,  the  displacement  portion  of  (3.4)  easily  yield 
these  three  unknowns  s^  ,  s^  ,  and  s^  . 

3.3.8  More  Difficult  Arrangements 

In  the  previous  examples,  the  existence  of  three  Intersecting 
revolute  axes  enabled  us  to  separate  the  problem  into  two  parts  - 
one  dealing  with  position  and  the  other  with  orientation.  The  two 
problems  were  then  solved  separately.  That  is  we  solved  a  three 
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*■■*»** 


degv ee-of-freedom  pcsition  problem  and  then  a  three  degree-of- freedom 
orientation  problem.  A  more  difficult  problem  is  one  in  which  position 
and  orientation  do  not  separate.  An  example  is  the  case  where  just  two 
revolute  axes  intersect.  Consider  the  6R  ,  a ^8282838^3^3^3^  mani¬ 
pulator  shown  in  Figure  3.10.  Here  axes  3  and  4  intersect.  The  vectors 
7P  ,  <}  ,  and  R  are  as  shown  in  Figure  3.10.  We  make  the  following 
observations: 


(3.93) 


(3.94) 


(3.95) 


1[7P]  -  2  -  R 


(3.96) 

(3.97) 


H7?]  •  *[7P]  -  a2  +  R 2  •  2S  •  R  (3.98) 


Then  using  (3.1)  for  the  A's  (3.93)  -  (3.96)  become  (taking 
81  "  s6  "  ■  0)  • 


l] 

n 

0 

0 

D 

0 

0 

0 

0 

n 

1 1 

0 

D 

D 

0 

I] 

0 

0 

D 


s3(c01s92sa2+seice2caisa2+s0lsalca2^ 

+a?(c0.c0o-s0ls02ca1)  +  s  2®  ©  ^Sfl,  ^  +  a1c01 

s3(s01s02sa2-c01c02C31sa2  - 
+a2(s91c02+c01s02ca1)  -  s2c01sa1  +  a1s@1 


83^”C02SaiSa2+CaiCa2^  +  a2s02sal  +  s2cal 

1 


a4(-c05c06+s05s06ca5)  -  a5c06  -  s5s06sa5 
+s4(-s05c06sa4  -  C05s06sa4ca5-s06ca4sa5) 


a4(c05s06+805C06Ca5)  +  a5S06  “  s5c06sa5 
+s4(s05s06sa4-c05c06sa.4ca5  -  c06ca4sas) 

-a4s05sa5  +  s4(c05sa4sof.5-ca4ca5)  -  s5ca5 


1 


^3 


c01s02sa2  +  s01c02ca1sa2  +  s01sa1ca2 
s01s02sa2  -  c01c02ca1sa2  -  c01sa1ca2 


c02sa^sor.2  +  ca1ca2 


0 


7 


s05c96sa4  +  c05s96sa4ca5 
-s05306so4  +  c05c06sa4ca5 
-c05sa4sa5  +  ca4ca5 


+  s06ca4sa5 

+  c06co4sa5 


0 


(3.99) 


(3.100) 


(3.101) 


(3.102) 
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In  addition  (3.99)  and  (3.100)  respectively  yield 

(£2  *  s^2  +  *2^  +  *2"  +  *1"  +  2a^a2C02  +  2ajS3S0t2S®2  2828500.2  (3.103) 

7.E?  *  «42  +  S52  +  «52  +  S42  +  2a^a5c65  +  2348^58(14  +  28485004  (3.104) 

Our  approach  to  this  problem  is  to  solve  £or  the  coordinates  (x,  y,  z)  of 
the  point  of  Intersection  of  axes  3  and  4.  With  this  in  mind  we 
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also 


(3.110) 


-1 


where  Aeq  Is  defined  as  in  equation  (3.53).  Thus  using  (3.100) 
for  2z  ,  (3.109)  for  ,  (3.108)  becomes 


<£2  +R2  *  2£i  .  R  -  (a42+ss2-ta524a42+2s4s<.ca4 

2“5 


a13x  +  a23y  +  «33z  +  a14_1  +  8^^5+85005 

[_  801 5 

We  next  want  to  express  m>3  and  -m,  in  terms  of  x  y  ,  and  z 
and  use  the  relation 

Uig  •  1114  *  c°sa.3  (3.112) 


2 

■  842'a42  +  «42  (3.111) 


For  this  we  need  cOj  ,  ,  c©2  ,  s02  ,  c05  ,  s0^  ,  c0^  ,  s0^  explicitly 

expressed  in  terms  of  x  ,  y  ,  z  .  We  note  '.02  and  s02  are 
simply  obtained  from  the  z-compcnents  of  (3.99),  and  from  (3.103) 
and  are 


>2  La2  _  ,3M2[z  -(.,cq14.,cq1^2)| 


2.i 


sai 


C02  ~ 


+  s 


3  **2' 


(3.113) 


,[z  ~(82ca^3cg^ca9^  +  83Sf*2  fc^  “(s32+a22'te22’+ai2+2s2S3ca2)| 

sai  2aj  L  J 


se„  ■ 


a22  +  832sa22 


(3.114) 


c0j  and  80 1  from  (3.99)  are,  after  simplification 


c0  _  x(838028a2+a2ce2+al)  “  y  [s3(c02c«l8C'2+8ntlca2>  “  a2s02cO. i  +  s2sO.  J 

+  y2 

(3.115) 


y(838028a2+a2c®2+al^+  x f83<ce2cai8a2'f8aica2)  "  «2se2cr*l  +  s2s<ill 

80i  ■  - - - = 

x2  +  y2 

(3.116) 

Where  we  may  use  (3.113)  and  (3.114)  for  c02  and  s02  .  When  (3.113) 
(3.114),  (3.115)  and  (3.116)  are  used  in  (3.101)  to  express  ^  in 
terms  of  x  ,  y  ,  and  z  ,  the  result  is  a  third  degree  expression  In 
x  ,  y  ,  and  z  .  If  we  do  similar  things  with  85  and  0^  for 
then  (3.112)  becomes  a  polynomial  of  degree  six  in  x  ,  y  ,  z  .  This 
along  with  (3.103)  and  (3.111)  are  three  equations  for  x  ,  y  ,  and 
z  .  However,  they  are  of  such  large  degree  that  finding  all  the  rocts 
is  not  feasible.  Though  there  are  some  special  cases  of  Interest. 

For  a  6R  ,  a2a^  manipulator,  with  “  a  3  °  (X5  “  90°  and 
a2  *=  <14  =  “90°  the  equations  reduce  to  a  degree  which  is  workable. 

This  configuration  is  shown  in  Figure  3.11.  Equation  (3.105)  reduces 
to 

x2  +  y2  +  z2  .  a2  (3.117) 

and  (3.111)  reduces  to 

x2  +  y2  +  z2  +  x^2  +  y^2  +  z^2  -  2xx4  -  lyyu  -  2z^z  -  a^2  (3.118) 
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where  R  in  (3.111)  has  been  replaced  by  its  components,  x4  »  3^  »  z4  » 
and  the  indicated  dot  product  performed. 

(3.99),  (3.100),  (3.101),  (3.102)  reduce  to 


a  * 


7 


P 


m  - 


a2c0jce2 

8288^82 

.2.e2 


•*4c95ce6’ 

*4ce5,t,6 

-*4,S5 


’-C0j802 

-S01802 

C0„ 


(3.119) 


(3.120) 


(3.121) 


from  (3.119)  we  obtain 


-S05c06 

80^s0g 

c95 

0 


C0J  -  x 

®2C®2 


s0!  -  .  y 

a2c02 


(3.122) 


(3.123) 

(3.124) 


802  * 


z 

a2 


(3.125) 
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using  (3.123),  (3.124),  (3.125)  in  (3.121) 


“3  «22c02 


-xy 


-yz 


a2  C02 


(3.126) 


replacing  c@22  by  1  -  s02  and  using  802  from  (3.125),  (3.126) 
becomes 


m3 


from  (3.120)  we  obtain 


2  [7-  z2“l  h 

L  a2  J 


-xz 


-yz 


-z2^22 


(3.127) 


c0# 


80f 


80c 


a4c05 

a4c95 

_7_ 


(3.128) 

(3.129) 

(3.130) 


substituting  (3.128),  (3.129),  and  (3.130)  in  (3.122)  and  simplifying 
gives  us 


Cu^] 


L  *2  _ 


-7  7 

y  z 
-Vz 

-7  2^  2 
z  +a4 


(3.131) 
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(3.132) 


We  now  rotate  7[uia]  to  express  it  in  terms  of  system  1  by 

1114  =  Aeq7[u^] 

with  Aeq  as  in  (3.53)  and  as  in  (3.131)  we  get 

-a117x7z  -  a127y7z  -  a137z2  +  a13a42 
m  -  1  .  -a2l7x7z  •  «227y7z  -  a237z2  +  a23a42  (3.133) 


7  7  7  7  7  2  2 

-a3l/x  z  -  a32  y  z  -  a33  z  +  a33a4 


To  eliminate  7x  ,  7y  ,  7x  from  (3.133)  we  use  (3.110)  with  Aeq  from 
(3.53)  which  after  simplification  yield: 

-(a13x-fa23y+a33z+a34  l)  (x-x4)  +  a13a42 

- 1 _ r  -(•13x+*23y+‘33z+*34'1)<y'y'.)  +  *23»42  (3.134) 

.42R  -  V  ’  1  9 

^7  -(a13x+a23y+a33z+a34-A)(z-z4)  -J- a33a4^ 


Then  using  (3.127)  for  m3  and  (3.134)  for  the  equation 

Ui3  •  Ji»4  *  0  results  in  the  polynomial: 

z(a13x+a23y4a33z-^34-1)(x2-+y24z2-xx4-yy4-zz4-a22-a42) 

l  -12,  2/  ,  .  .  -1.  .  2  2 

+  za34  a4  +  z4a24(al3x+a23y-*33z+a34  )  +  a2  a4  a33  - 


(3-135) 


We  note  that  linear  combinations  of  the  equations  (3.117),  (3.118)  and 
(3.135)  can  be  formed  to  reduce  the  degree  of  the  equations. 

Equation  (3.117)  we  leave  as  is.  Combining  (3.117)  with  (3.118) 
leads  to 

2x4x  +  2y4y  +  z  +  a42  -  a^  -  (x^-ty^-fz^)  -  0  (3.136) 

and  using  (3.136)  and  (3.117)  in  (3.135)  yields 


n  ■  ».[.13A!  .  x42  1  >42  +  *42  -  Iii>] 

2  2 

.2  ..  2  ^  2 

+  vrFa23  * 


*[.„<-  3-  -  '**’K.+.Y  .  V  >] 


2 

2 


+  z2[a33(— 


X/2  +  w .  2  4., ,2 


it-. -zi.  .  fi_ 

2  2  ' J 


+  x  (ai3z4a2  > 
23zUa2 


+  y  (a„z,a,2) 


(3.137) 


+  z  C(a33z4a22)  +  a34_V  f  a34  <_  ~2 


2  -1  .  2a  2 
+  z^a2  «34  +  82  a33 


r-  tlL  .  X42  +  y42  +  z42  _  f4^ 


)] 


Hie  equations  (3.117),  (3.136)  and  (3.137)  are  three  equations  for 
x  ,  y  ,  and  z  .  The  linear  equation  (3.117)  can  be  used  to  eliminate 
one  variable  easily.  Another  variable  can  be  eliminated  between  (3.136) 
and  (3.137)  leading  to  a  polynomial  of  degree  four.  Hiis  procedure 
has  been  carried  out  and  programmed  on  the  PDP-6.  An  analysis  program 
was  used  to  generate  Inputs  with  known  angles  to  check  the  results. 

A  typical  example  was  generated  by  the  arbitrary  input  angles 

»1  -  34°  ,  e2  •  21°  ,  03  -  78°  ,  @4  --56°  ,  85  -  23°  ,  86  -  1°  and 

link  parameters  «2  "  a4  “  *5"  »  which  gave: 


Aeq 


-0.322 

-0.481 

0.816 

12.066 

0.555 

0.641 

0.577 

18.035 

-0.801 

0.598 

0.037 

-5  609 

0 

0 

0 

1 
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For  the  above,  the  four  sets  of  common  roots  were  real  and  lead  to 
four  sets  of  angles  for  each  root.  They  are  shown  in  Table  3.1. 

If  a^O  then  the  solution  may  first  be  expressed  as  three 
quadratic  equations  in  three  unknowns  (x  ,  y  ,  and  z)  and  finally 
as  an  eighth  degree  polynomial.  With  a^O  ,  a2»*0  >  a^O  ,  and  a'a 
as  before,  and  with  x  ,  y  ,  z  defined  as  before,  (3.105)  becomes 

(x2  +  y2  +  z2  -  ai2  -  a22)2  +  4aj2  ( z 2  -  a22)  *  0  (3.138) 

and  equation  (3.111)  reduces  to  (3.118)  as  previously  noted.  To 
form  we  use  C®1  and  s0^  from  (3.115)  and  (3.116).  Next 
we  use  c02  and  s@2  from  (3.113)  and  (3.114)  and  substituting 
these  quantities  into  (3.101),  we  obtain  after  simplification: 

-  2xz 

-  2yz 

-  2z2  +  (x2+y2+z2-a22)  +  2a22  - 
0 

(3.139) 

is  as  belore  and  given  in  (3.134).  By  using  (3.139)  and  (3.138) 
in  (3.112),  by  replacing  (x2+y2h”.2)  with  its  equivalent  from  (3.118), 
and  by  simplifying,  (3.1)2)  becomes: 

0  =  x2(2a13z4x4) 

+  y2(2a23z4y4) 

+  z2Ca33(-R2-a424ai2-a22)  +  2a33z42] 

+  xy[2a13z4y4  +  2a23z4x4] 

+  yzL2a23z42  +  a23(- R  ^a^+aj2^2)  +  2a33z4y4] 


1 

Ul3  *  - 

2a2[(ai‘f^1  (x2ty2+z2"a22-ai2)] 
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2  ?222 

+  zx[2a33z4x4  +  2a13z4  +  a13<“£  _a4  “ax  )] 

+  x[a13z4(- R2+a42-ai2^22^  +  2a33a42*4  +  2a34_lz4x4] 

+  yCa23z4<"  R2+a42-ai24a22)  +  2a33a42y4  +  2a34lz4y4^ 

+  z[a33z4(-  R2+a42-a12-^22)  +  2a33a42z4  +  a34_1(2z42"  £2  +  a42 
+  al2  -  a22)] 

+  Ca34‘lz4<-*2V-alS2>  +  a33a42(“^2V-alS2)]  * 

(3.140) 

When  (3.118)  is  used  for  x2  +  y2  +  z2  in  (3.138),  that  equation  becomes 
quadratic.  This  together  with  (3.118),  and  (3.140)  are  the  three 
quadratics  for  x  ,  y  ,  and  z  .  Eliminating  two  variables  produces 
a  single  polynomial  of  degree  eight.  The  preceding  was  programmed  on 
the  PDP-6  to  yield  a  final  polynomial  In  z  .  For  the  link  parameters 
8^13  ,  a2-15  ,  a4"15  ,  several  examples  were  run.  Examples  were  found 
In  which  eight  sets  of  values  did  Indeed  satisfy  the  three  quadratics. 

One  of  these,  generated  in  the  Input  angles  0j*9°  ,  02*175°  ,  03*188P, 
04*173°,  03**174o,  0^*169°,  led  to  the  following  set  of  elbow  positions: 


X 

y 

z 

1 

-25.342 

11.820 

1.048 

2 

-24.457 

-13. 534 

1.200 

3 

-1.914 

-0.569 

0.294 

4 

-1.919 

-0.304 

1.307 

5 

-1.960 

0.399 

-0.019 

6 

-1.979 

-0.168 

-0.641 

7 

-12.297 

0.735 

14.985 

8 

-18.119 

1.073 

-14.088 

Now,  in  order  to  extend  the  above  problem  to  include  a3/0  ,  we 
must  define  a  new  variable 

W  -  x2  +  y2  +  z2  (3.141) 
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We  then  replace  the  terms  (x^+y^-hs^)  with  W  in  (3.105)  (3.111), 
and  (3.112)  and  appropriately  rewrite  .  Equations  (3.105),  (3.111), 
(3.112),  and  (3.141)  become  quadratic  in  w  ,  x  ,  y  ,  and  z  .  The 
details  of  this  may  be  found  in  Appendix  V.  According  to  Bezout's 
theorem  this  system  has  at  most  16  sets  of  common  roots.  However,  no 
method  is  known  by  which  three  of  the  variables  may  be  eliminated  to 
attain  one  polynomial  of  only  degree  16. 

To  summarize  the  above  we  have  found  that: 

1.  A  6R,  &2ai  rosy  have  as  many  as  four  different 
configurations  leading  to  the  same  hand  position 
and  orientation. 

2.  A  6r,  8^2*4  may  have  as  many  as  eight  different 
elbow  positions  (the  elbow  is  considered  to  be  the 
point  of  intersection  of  axes  3  and  4)  leading  to 
the  same  hand  position  and  orientation. 

3.  A  6R,  a^a2a^a^  will  have  at  most  16  different 
positions  that  the  elbow  can  assume  for  each  fixed 
hand  position  and  orientation. 


C’lAPTER  IV 


NUMERICAL  SOLUTIONS 


Our  solutions  so  far  have  been  made  possible  by  the  existence  of 
special  geometry.  To  analyse  more  general  cases,  iterative  procedures 
must  be  used..  Two  procedures  are  presented  to  handle  these  cases. 

The  first  employs  the  well-known  Newton-Raphson  technique*  and  the 
second  applies  velocity  methods. 


4 . 1  Newton-Raphson 

The  Newton-Raphson  method  assumes  the  existence  of  an  appro'  imate 
solution.  Then  the  equations  are  linearised  and  an  increment  to  this 
approximation  is  computed  hopefully  leading  to  a  more  accurate  approxi¬ 
mation.  We  write 


e 


i 


lo 


i  -  1 . 6 


(4.1) 


where  g^Q  is  the  first  approximation,  and  60^  Is  the  increment,  and 


is  the 


more  accurate  approximation. 

We  may  then  write 

(3.4)  as 

c((>i0+iS  4) 

-«(9i0+  fiscal 

sfA  +  60  )sa. 
io  i  i 

a.cffl.  +6fi  )“ 

i  io  1 

»(Sio+  {ty 

c'6i0+  ffi^cai 

-c'Clo+ 

a  s(0  +  60  ) 

i  io  i 

0 

sai 

o 

p 

H* 

8i 

0  0  0  1 


(4  2 


*Thia  method  was  applied  to  seven  link  mechanisms  with  revolute  pairs 
by  Uicker,  Denavit,  and  hartenberg  L49J.  The  approach  presented  here 
is  similar  to  theirs.  They  assumed  the  motion  of  one  link  to  be 
prescribed  as  an  input  and  found  the  displacement  of  the  rest  of  the 
mechanism  as  they  incremented  the  input. 
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Then  expanding  c(0.  +  60  )  and  s(0  +  60  )  ,  using  trigonometric 

io  i  io  i 

Identities  for  the  sum  of  two  angles  and  letting  c(®0  )=1,  s(60  )=  60 

i  11 

(4.2)  becomes 


c  9 

io 

-8  0  Ctt 
io  i 

8  0  6U 
io  i 

a  c  0 
i  io 

■s  © 

io 

-c®  ca, 
io  i 

c0.  sa 
io  i 

-a,  s0 
i  io 

Ai  ■ 

8  0. 
io 

c0  ca 
io  i 

-c0  sa 
io  i 

a  s  6 
i  io 

60. 

+  i 

c  0 

io 

s0  ca 
io  i 

sG  sa 
io  i 

ai^io 

0 

8<1i 

CCti 

s 

1 

0 

0 

0 

0 

0 

0 

0 

1 

0 

0 

0 

1 

(4.3) 

which  we  write  as 

A  +60 
io  i 

B 

io 

(4.4) 

where  A,  and  B  are  defined  from  (4.3).  Using  (4.4)  in  the  basic 

io  io 

eqn.  (3.4)  and  retaining  only  terms  of  degree  one  or  less  In  60^  we 


have 


6^  (B10A20A30A40A50A60) 

+  6^  (a10B20A30A40A50A6(P 

+  683  (A10A20B30A40A50A60)  ..  ... 

&  (4.3) 

+  4  ^ A 10A20A30B40A  50A60  ^ 

+  fl95  (A  ^20^0^0  B5(^  60  ^ 

+  ^6  ^A10A20A30A40A50B60^",Aeq"A10A20A30A40A50A60 


The  matrix  equation  (4.5)  contains  six  independent  linear  equations 

that  may  be  used  to  compute  , (i*l . 6>  .  It  is  noted  that  the 

i 

preceding  was  developed  for  revolute  Joints,  but  the  method  is  alsr 
applicable  to  manipulators  with  prismatic  joints  provided  appropriate 
changes  are  made  in  the  B 

io 

This  method  lends  itself  to  computation  on  a  digital  computer.  A 
program  has  been  written  implementing  this  scheme.  The  inputs  to  the 
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program  are  the  joint  angles  of  the  manipulator  in  its  initial  state, 
and  the  desired  final  hand  position  and  orientation.  The  output  in  a 
set  of  angles  leading  to  the  final  state.  If  the  final  state  is  a  great 
deal  different  from  the  initial  state,  then  solutions  of  C4.5)  will  not 
yield  "small"  corrections  and  the  method  will  not  converge.  In  order 
that  (4.5)  be  valid  it  is  necessary  to  generate  intermediate 
targets.  The  right 'hand  side  of  (4.5)  represents  a  translation  and  a 
rotation.  Intermediate  goals  are  specified  by  taking  a  fraction  of 
the  total  rotation  and  translation.  Tht>  program  begins  with  the  initial 
angles  as  the  first  approximation.  Then  it  computes  the  next  approxi¬ 
mation  based  on  an  intermediate  goal.  A  new  intermediate  goal  is  computed 
and  the  process  continpes  until  a  satisfactory  set  of  angles  is  found  or 
the  method  falls  to  converge  after  a  fixed  number  of  iterations.  See 
Appendix  I  for  details. 

4.2  Iterative  Velocity  Method 

The  iterative  velocity  method  is  based  on  the  fact  that  a  change  in 
position  and  orientation  of  a  rigid  body  (in  this  case  the  hand)  can  be 
expressed  as  a  screw  -  a  rotation  about  and  a  translation  along  a  single 
fixed  axis.  In  addition,  for  small  motion,  it  can  be  shown  that  the 
screw  is  related  to  the  angular  velocity. 

We  write  W  and  V  as  approximations  respectively  to  the  angular 
velocity  of  the  hand  and  the  linear  velocity  of  a  point  in  the  hand  at 


*0n  the  right-hand  side  of  (4.5),  Aeq  represents  the  desired  position  and 
the  product  of  the  six  matrices  represents  the  present  position.  Hence, 
the  difference  gives  the  displacement  which  may  be  represented  as  a 
rotation  and  a  translation. 


the  origin: 


(4.6) 


W  =  n 
A  t 

V  =  h  tig  n  -  n  x  £  Acp  ,  (4.7) 

At  At” 

where  quantities  on  the  left-hand  side  of  the  above  are  found  from  the 
screw;  A^  is  the  amount  of  rotation,  n  is  a  unit  vector  parallel 
to  the  screw  axis,  ''  is  the  pitch  of  the  screw,  and  £  is  a  vector 
from  the  origin  to  the  screw  axis.  The  details  are  shown  in  Appendix  II. 
In  addition  we  may  express  the  angular  and  linear  velocity  as  functions 
of  the  rotations  in  the  arm  joints.  That  is 

6 

W  -  W  (4.8) 

i-1  i 

6_ 

V  «-2  w  X  r  (4.9) 

-  i-i  "i  "i 

where  is  the  angular  velocity  of  the  hand  due  to  the  rotation  about 

axis  i  and  £^  is  a  vector  from  the  origin  of  system  1  to  axis  i. 

We  make  the  approximation 


”i 


A*. 

Tt 


(i-1 . 


(4.10) 


where  n^  is  a  unit  vector  parallel  to  axis  i  and  we  assume  that  the 
motion  of  the  hand  from  initial  to  final  position  is  small,  so  that 
<'4. 81  and  <'4.9)  may  be  written  using  ('4. 10)  as 

6  A  9  . 

W  =  /  - 1  n  (4,11) 

-  %  At  "I 

c  A  0  i 

V  -  -> - 1  n,  x  r  (4.12) 

’  ‘i*i  At  "I  "i 
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Then  equating  the  right  hand  sides  of  '4. 11)  and  tU.12)  to  the  right- 
hand  sides  of  (4,6)  and  (4,7),  and  we  obtain  two  vector  equations  repre¬ 
senting  six  scalar  linear  equations  in  A0^  ,  i-1,..., 6.  Equating  and 
dividing  by  At  yields: 

6, 

rA0  tO  -  A®n  (4.13) 

i-1  1 

6 

("AS  n  x  r  )  «  H  A®  n_  -  n_  x  r  ^q>  (4.14) 

i-1  i  “i  “i 

The  right-hand  sides  of  (4.1)  and  (4.14)  are  computed  from  the  known 
changes  in  position  and  orientation  of  the  hand.  Since  the  initial 
configuration  of  the  manipulator  is  known,  we  have  values  for  the 
and  r^  .  As  long  as  the  changes  in  position  and  orientation, 
as  represented  by  the  screw,  are  small,  then  the  solution  of  this  set 
of  equations  gives  small  changes  in  the  joint  angles.  Thus  the  r^ 
and  the  n^  do  not  change  very  much  and  we  are  justified  in  using 
their  values  in  the  initial  state.  To  apply  this  method  we  must  insure 
that  the  right-hand  sides  6f  these  equations  are  small.  Therefore, 
for  large  motions,  we  take  only  a  portion  of  the  screw  to  compute 
the  incremental  change  in  the  angles.  We  also  limit  the  change  that 
is  made  at  each  iteration. 

If  any  of  the  revolutes  are  replaced  by  prismatic  joints,  this 
method  may  still  be  applied  with  appropriate  changes  in  (4.11)  and 
(4.12). 

A  computer  program  has  been  written  utilizing  the  above  scheme, 
details  of  which  can  be  found  in  Appendix  II. 
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4.3  Comparison  of  the  Methods 

It  was  desirable  to  test  and  compare  these  methods  to  determine 
their  practicability  In  finding  solutions  for  complex  manipulators.  In 
addition  It  was  hoped  that  the  velocity  method  would  be  faster  as  It  does 
not  require  the  matrix  multiplication  that  the  more  conventional 
Newton-Raphson  method  does.  A  6R,  82828384  manipulator  was  used  as  a 
trial  for  the  two  methods.  For  the  purpose  of  testing,  the  target  hand 
positions  were  generated  by  sets  of  known  angles.  Programs  were  written 
in  FORTRAN  IV  for  the  PDP-6.  With  this  machine,  an  iteration  using 
Newton-Raphson  took  0.140  seconds  while  the  velocity  method  took  0.097 
seconds.  A  typical  example  Is: 

With  the  parameters  of  the  arm  fixed  at 

ai  -  a3  -  a5  -  90° 

a2  -  a4  -  -90° 

&2  -  0.375 

s3  -  12.2 

a3  -  0.375 

s  -  9.5 
5 


and  arbitrarily 


5.9 


V°  • 

the  target  was  generated  by  the  angles 
9  — o  u  - o 


.  #  .1°,  6  -  50°, 

3  4  5  ’6 

This  leads  to  the  hand  position  specified  by  position  vector 


9  -  100  ,  9  -  100  ,  9  -  30°,  6  -  30  ,  6  -  50  ,  H  -  0 
1  2  3  4  *  5  6 


£2- 


27.915 

-0.869 

0.314 
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and  the  orientation  (specified  by  two  unit  vectors  fixed  in  the  hand 


L2  pointing  in  the  direction  of  the  hand  and  N^  in  the  direction  of 
the  sixth  revolute  axis) ; 


-0.869“ 

0.314 

L2“ 

0.492 

-0.042 

,  n2  - 

0.486 

-0.816 

The  initial  configuration  of  the  arm  was 

\  -  70°,  -  80°,  -  40°,  6  -  0,  8  -  60°,  9  -  30° 

1  z  3  4  5  6 

with 


14.229 

-0.979 

-0.105 

20.295 

L.  ■ 

0.196 

N,  - 

-0.220 

14.141 

-1 

» 

0.061 

-1 

-0.970 

The  velocity  method  resulted  in: 

9t  -  ioo.oo,  02  -  100.00,  93  -  30.00,  e4  -  30.00,  05  -  50.00,  06  - 


27.914 

-0.870 

0.313 

21.001 

*  L,  " 

0.492 

N  - 

0.486 

24.863 

"2 

-0.042 

“2 

-0.816 

Number  of  Iterations  -  10 

Run  time  *  0,97  seconds 


The  Newton -Raphson  method  resulted  in: 

0.  -  100.00  9,  -  100.00  9  -  30.02  9  -  30.00  6  -  49.98  9  - 

1  ‘  3  4  5  ® 


27.914 

-0.870" 

"0.314“ 

21.002 

L  ■ 

0.492 

n  " 

0.486 

24.862 

-2 

-0.042 

-2 

-0.816 

Number  of  iterations  ■  13 


Run  time  ■  1.82  seconds 


From  the  results  of  many  tests  similar  to  th^  above  the  following 


was  observed: 


71 


0.00 


0.01 


f* 


1.  For  small  motions  (rotations  of  about  10P  In  each  joint)  both 
methods  converged  to  solutions  but  the  velocity  method  generally 
had  fewer  iterations. 

2.  For  larger  motions  (rotations  of  10°  -  90°  in  each  joint), 
Newton-Raphson  did  not  always  converge  within  the  upper  limit 
of  400  Iterations.  The  velocity  method  did  in  all  cases  tested. 

3.  For  even  larger  motions,  the  velocity  method  did  not  always 
converge  within  400  iterations  but  did  converge  in  all  examples 
when  allowed  more  than  400  iterations,  ’lowever,  such  was  not 
the  case  with  Newton-Raphson.  In  some  examples  even  after 

4000  iterations,  it  still  did  not  converge. 

Both  methods  become  very  time  consuming  whenever  the  course  of  the 
solution  takes  the  arm  to  the  equivalent  of  a  "stretched  out"  position. 
That  is  whenever  the  hand  is  in  a  position  from  which  it  cannot  move 
in  an  arbitrary  direction  and  rotate  about  an  arbitrary  axis,  the 
system  of  equations  formed  in  both  the  above  methods  degenerates. 
Generally,  these  methods  work  their  way  out  of  such  predicaments  by 
taking  very  small  steps,  and  by  benefiting  from  round-off  error  inherent 
in  the  computations.  The  further  the  distance  between  initial  and 
final  states,  the  more  degeneracies  that  are  likely  to  be  encountered 
entente  to  the  final  state. 

Even  though  convergence  is  occasionally  slow,  the  velocity  method 
reached  a  solution  in  all  cases  tested,  thereby  proving  it  to  be  useful 
for  complex  manipulators.  In  particular  for  a  short  range  of  motion  it 
was  very  efficient.  Thus  it  might  be  used  most  effectively  to  find 
the  final  set  of  angles,  when  a  first  approximation  has  been  obtained 
using  a  rather  simplified  model  of  the  manipulator. 
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CHAPTER  V 


A  DIGITAL  MANIPULATOR 

5-1  Description  of  the  Manipulator 

One  type  of  manipulator  whose  solution  does  not  fall  into  the 
class  previously  discussed  is  one  containing  more  than  six  degrees-of- 
freedom  but  having  a  limited  motion  in  each  joint.  An  articulated 
arm  of  this  type  having  many  degrees-of-f reedom  was  described  by  Anderson 
and  Horn  [  3  7  3 .  They  found  that  such  a  design  was  practical  for  use  in 
an  underwater  laboratory.  In  fact,  they  claim  that  this  design  opti¬ 
mized  many  desirable  criteria  such  as  slenderness,  cost,  microdexterity 
and  range  of  operation. 

If,  in  addition  to  restricting  the  range  of  freedom  at  each  joint, 
we  allow  only  a  finite  number  of  states  to  exist  at  each  joint,  then  the 
arm  becomes  digital  in  nature.  This  makes  it  easy  to  be  interfaced  with 
a  digital  computer.  The  concept  of  such  an  arm  was  suggested  by 
L,  Leifer  who  together  with  V.  Scheinman  developed  working  models  tor 
the  Stanford  Artificial  Intelligence  Project  (see  Figure  5.1).  Since 
the  arm  is  snake-link  in  form,  they  named  it  the  "ORM"  (the  Norwegian 
word  for  snake) 

We  shall  examine  the  problem  of  finding  a  solution  for  a  digital 
arm  It  can  be  seen  that  knowledge  of  the  state  of  each  joint  together 
with  knowledge  of  the  link  geometry  is  sufficient  to  specify  the  position 
of  the  hand.  The  orientation  freedom  of  this  device  is  limited  In 
practice  it  would  need  to  have  a  wrist  capable  of  putting  the  hand  in 
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the  proper  orientation.  We,  therefore,  examine  the  problem:  given  the 
desired  position  of  a  point  in  the  hand,  to  find  the  state  of  each  joint 
leading  to  this  position. 

The  problem  is  divided  in  two  parts.  The  first  is  to  consider 
a  two-dime  isional  arm  and  develop  a  technique  to  solve  it.  The  second 
is  to  develop  a  method  for  three  dimensions  with  insight  gained  from 
looking  at  the  two-dimensional  problem. 

5.2  Two-Dimensional  Model 

The  two-dimensional  or  planar  arm  to  be  considered  is  binary  in 
nature.  In  other  words,  there  are  only  two  states  for  each  joint.  If 
the  arm  is  made  up  of  n  lin«c3,  there  are  2n  possible  configurations. 
A  model  of  this  arm  is  shown  in  Figure  5.2,  where  the  angle  between  two 
adjacent  links  can  be  either  +  0Q  or  -8Q  where  8q  is  a  constant. 


Figure  5.2.  Binary  Arm  . 

If  we  number  the  joints  1 . .  and  denote  the  rotation  in  the  i  — 

joint  by  8^  ,  i-1 . n,  our  problem  is  to  find  the  9^  such 

that  the  end  of  the  final  link  is  “close"  tb  the  desired  target. 

Since  there  are  exactly  2n  P088ible  configurations,  there  are 
at  most  2n  points  that  the  end  of  the  arm  (the  hand)  can 
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reach.  Thus,  in  general,  the  hand  cannot  be  placed  at  an  arbitrary  point. 
Hopefully,  however,  with  sufficient  links  the  hand  could  be  placed  close 
to  any  arbitrary  point  within  its  workspace. 

There  exists  a  well-defined  transformation  ^see  Appendix  III) 
to  find  the  position  of  the  hand,  given  the  0^  .  However,  the  inverse 
problem  (i.e.,  given  the  hand  position,  find  the  associated  9^)  has 
yet  to  be  solved.  Theoretically,  we  could  exhaustively  examine  the 
2n  possibilities  construct  a  table  and  then  choose  the  one  that  places 
the  hand  closest  to  the  target,  but  in  practice  this  would  be  too  time 
consuming.  We  therefore  need  a  systematic  method  to  help  in  dealing  with 
such  a  large  solutibn  space.  If  we  define  the  error  as  the  Euclidian 
distance  of  the  hand  from  the  target,  the  scheme  outlined  in  Figure  5.3 
suggests  itself. 


FINISH 

Figure  5.3.  Sequential  Search  Procedure. 

In  this  method  the  arm  is  put  initially  into  some  arbitrary  con¬ 
figuration  and  the  position  of  the  hand  computed.  Starting  at  the 
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origin  each  joint  is  examined  sequentially  to  see  if  the  other  state 

at  that  joint  would  reduce  the  error.  If  it  does  reduce  the  error, 

the  change  is  made.  That  is,  the  rotation  at  that  joint  is  considered 

to  be  reversed  and  the  position  of  the  hand  computed  while  the  rest 

of  the  arm  remains  rigid.  The  state  of  that  joint  is  then  changed 

if  necessary  to  reduce  the  error.  However,  the  existence  of  local 

minima  prevent  convergence  of  this  method  in  many  cases.  It  is 

possible  to  get  improvement  by  using  look-ahead.  Instead  of  considering 

changing  each  joint  singly,  the  results  of  changing  that  joint  along 

with  changes  in  the  next  k  joints  are  considered.  This  may  be 

called  k-stage  look-ahead,  and  would  involve  computing  the  hand  position 
k+1 

2  times  for  each  joint.  There  are  now  many  strategies  possible  using 
combinations  of  0,1,2....  -  stage  look-ahead.  There  is,  oi  course, 
a  trade-off  between  the  amount  of  look-ahead  and  computation  time. 

For  instance,  one  strategy  was  to  use  no  look-ahead  until  the  error 
could  not  be  reduced,  then  try  1-stage  until  no  improvement  resulted 
then  2-stage  etc.  The  process  was  halted  if  the  error  was  sufficiently 
small  or  the  look-ahead  became  too  large  (usually  3-stage  was  as  much 
as  was  allowed) . 

For  purposes  of  trying  these  strategies,  an  arm  with  twenty-four 
1  inch  links,  with  possible  rotations  of  -15°  in  each  joint  was 
modeled  in  the  computer.  Tests  were  started  with  the  arm  extended 
along  the  x-axis  as  shown  in  Figure  5.4.  The  results  are  presented  in 
Table  5.1.  Computation  times  are  shown  in  Table  5.2. 
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TAR-  NO  LOOK-AHEAD  I  1-STAGE  LOOK-AHEAD  2-STAGE  LOOK-AHEAD  NO  LOOK-AHEAD  UNTIL  NO  IMPRCVE- 

GET  CONTINUED  UNTIL  NO  I  CONTINUED  UNTIL  NO  CONTINUED  UNTIL  NO  MENT  FOLLOWED  BY  1 -STAGE  LOOK- 


y 


Figure  5.4,  The  arbitrarily  chosen  initial  starting 
configuration  for  the  arm. 


It  can  he  seen  that  no  strategy  tried  was  best  in  all  cases.  These 
methods  had  the  additional  disadvantage  that  the  shape  of  the  arm  itself 
was  not  predictable.  It  was  hoped  that  improvement  would  result  if  the 
searching  was  started  after  the  hand  was  put  at  a  point  near  the  target 
by  some  simple  procedure. 

In  order  to  place  the  hand  near  the  target,  a  curve  connecting 
the  origin  and  the  target  was  generated,  whose  arc  length  was  equal  to 
the  length  of  the  arm,  and  whose  curvature  did  not  exceed  that  which 
the  arm  could  assume.  Curves  made  up  of  segments  of  four  circles  having 
the  above  properties  were  used  (details  of  the  derivation  of  these 
circles  are  presented  in  Appendix  III),  and  a  rought  attempt  was  made 
to  match  the  arm  to  this  curve.  After  the  rough  curve  match,  the 
previously  described  searching  technique  with  l-stage  look-ahead  was 
used.  Various  curve  matching  algorithms  and  different  radius  circles 
were  used.  Some  of  the  results  are  shown  in  Figures  5.5  through  5.9. 

Figure  5.5  shows  the  configuration  resulting  after  four  loops  with 
no  look-ahead,  then  two  loops  of  1-stage  look-ahead.  The  procedure  was 
started  with  the  arm  aligned  along  the  x-axis  as  in  Figure  5.4. 

Figure  5.6  is  the  result  of  first  matching  the  arm  to  a  curve  composed 
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of  segments  of  four  circles,  and  then  using  two  loops  of  1-stage  look¬ 
ahead. 

Since  the  arm  can  tilt  -15°  at  each  joint  and  the  joints  are  a 
fixed  distance  apart  the  arm  bends  in  a  circle  if  all  the  tilts  are  in 
the  same  direction.  The  radius  of  this  circle  is  the  minimum  that  the 
arm  can  assume.  In  Figure  5.6,  the  radii  of  the  circles  used  to  generate 
the  circle  segment  curve  have  this  minimum  radius.  To  match  the  arm 
to  the  curve,  the  state  of  joint  i  was  chosen  so  that  joint  i+1 
on  the  arm  was  as  close  as  possible  to  point  i+1  on  the  circle-segment 
curve.  As  can  be  seen,  this  procedure  definitely  influenced  the  shape 
of  the  final  result.  It  may  be  noted  from  Figure  5.6,  that  in  the 
attempt  to  match  the  arm  to  the  circle -segment  curve  the  arm  lagged  the 
Curve.  One  attempt  to  remedy  this,  was  touse  larger  radii  circles  to 
generate  the  curve.  It  can  be  seen  from  Figure  5.7  that  this  improved 
the  match. 

To  reduce  the  lag  even  further,  the  state  at  joint  i  was  chosen 
so  that  joint  i+1  was  as  close  as  possible  to  point  i+2  on  the  curve. 
This  appeared  successful  as  can  be  seen  in  Figure  5.8.  An  attempt  to 
match  the  arm  to  the  curve  in  both  slope  and  position  was  made. 

Figure  5.9  shows  the  results  of  this  scheme.  The  arm  womewhat  took 
on  the  shape  of  the  curve,  but  not  as  much  as  in  the  other  schemes. 

The  results  presented  were  for  the  target  (10,10).  However,  they 
are  similar  to  those  obtained  for  other  targets.  The  best  results 
were  obtained  when  the  radii  of  the  circle  segments  were  larger  than 
the  minimum.  For  radii  of  too  great  a  magnitude,  no  curve  existed  that 


Figure  5.5.  Result  of  arm  after  4  loops  of  0  look-ahead  and  two  loops 
of  l-stage  look-ahead.  The  starting  configuration  was 
along  the  x-axls,  as  in  Fig.  5.4. 
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Figure  5.6.  Result  of  trying  to  match  arm  to  curve  made  up  of  segments 
of  four  circles,  and  the  improvement  after  two  loops  of 
1-stage  look-ahead.  Radii  of  circles  is  equal  to  minimum 
radius  that  the  arm  can  assume.  The  curve  matching  tech¬ 
nique  was  to  choose  ©;  so  that  point  i+1  on  the  arm  was 
as  close  as  possible  to  point  i+1  on  the  curve. 


Figure  5.8.  Result  of  trying  to  match  arm  to  curve  made  up  of  segments 
of  four  circles,  and  improvement  after  two  loops  of  1-stage 
look-ahead.  Radii  of  circles  is  1.2  times  minimum  that 
arm  can  assume.  The  curve  matching  technique  used  was  to 
sequentially  choose  9i  so  that  point  i+1  on  the  arm  was  as 
close  as  possible  to  point  i+2  on  the  curve. 
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Points  on  curve  made  up  of 
circle  segments 

Arm  after  attempt  to  match  curve 

Arm  after  two  loops  of  sequential 
1-stage  look-ahead 


Igure  5.9.  Result  of  trying  to  match  arm  to  curve  made  up  of  segments 
of  four  circles,  and  Improvement  after  two  loops  of  1-stage 
look-ahead.  Radii  of  circles  is  equal  to  minimum  radius 
arm  can  assume.  The  matching  technique  was  to  sequentially 
choose  ©,  so  that  (|iu^L_£J  was 

a  minimum,  where  (x^.y,)  is  the  coordinate  of  joint  i  on 
the  arm  and  (*k  ,  yk)  is  the  coordinate  of  point  i  on  the 
curve.  This  matching  criteria  puts  a  weight  on  the  slope  as 
well  as  the  position  of  the  links. 
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could  connect  the  origin  and  the  target  point.  A  value  of  1.2  times 
the  minimum  seems  to  be  about  optimum.  It  was  found  that  this  allows 
for  a  measure  of  control  over  the  final  shape  of  the  arm  as  well  as 
generally  reducing  the  position  error. 

5.3  Three-Dimensional  Model 

This  arm  is  similar  to  its  2-dimensional  counterpart.  The  differ¬ 
ence  is  that  two  axes  of  rotation  exist  at  each  joint.  The  axes  inter¬ 
sect  and  are  90°  apart.  (See  Figure  5.10.)  We  assume  our  model  to  be 
constructed  so  that  eight  states  are  allowed  at  each 

AXIS  1 


Figure  5.10,  Typical  joint  in  3-dimensional  digital  arm. 

joint.  These  are:  either  a  rotation  of  -t-8  about  axis  1  with  none 

-  o 

about  axis  2,  or  a  rotation  of  ±9  about  axis  2  with  none  about 

o 

axis  1,  or  rotations  about  each  such  that  the  net  result  is  a  rotation 

-tw  about  axe3  midway  between  axes  1  and  2.  Thus,  two  links  can  be 

o 

tilted  with  respect  to  one  another  tB0  about  4  different  axes  that  are 
45°  apart.  If  we  denote  the  rotation  about  axis  1  as  8  and  the  rota¬ 
tion  about  axis  2  as  V  ,  then  the  possible  states  are: 
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State 

9 

<P 

1 

+  9 
o 

0 

2 

-9 

o 

0 

3 

0 

+  0 
o 

4 

0 

■0o 

5 

+tan  *(~  tanB  ) 
•42  ° 

sin'^-^sinS  ) 

•J  2  ° 

6 

"1  i 

+tan  (— tan0  ) 
s/ l  o 

-sin'1(sJ^sin0o) 

7 

"tatl  l(j2t&n6o) 

-sin‘1(^-aln0  ) 

s/2  o' 

8 

-tan  1(^tan9  ) 
•42  ° 

+sin“^(4-sin8  ) 
s/  2  ° 

where  the  values  of  9  and  <p  In  states  5-8  are  the  actual  rotations 
about  axes  1  and  2,  leading  to  equivalent  tilts  about  axes  at  45° 
to  1  and  2  (see  Appendix  III).  An  n-link  arm  would  then  have  8° 
possible  configurations.  Again  there  is  a  well-defined  transformation 
to  find  the  position  of  the  hand  given  the  angles  (See  Appendix  III) 
but  no  such  transformation  exists  to  find  the  angles  given  the  hand 
position. 

The  procedure  presented  in  Figure  5.3  is  still  applicable  except 

8  states  exist  at  each  joint.  Then  each  joint  would  be  examined 

sequentially  and  the  state  at  that  joint  is  chosen  which  minimizes 

the  Euclidian  distance  of  the  hand  from  the  target.  With  k-stage 

look-ahead  variations  in  the  jotnt  under  consideration  plus  all 

possible  combinations  of  the  next  k  are  considered.  Then  only  the 

joint  under  consideration  is  moved.  The  position  of  the  hand  must  thus 
k+1 

be  computed  8  times  for  each  joint  and  the  time  involved  in  these 
computations  will  limit  the  amount  of  possible  look-ahead. 
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An  arm  with  24  1-ir.ch  links  and  possible  tilt  of  ^15°  between  each 
link,  was  modeled  in  the  computer.  The  arm  was  placed  initially  in 
the  configuration  of  Figure  5.3  and  strategies  involving  combinations 
of  no  look-ahead  and  1-stage  look-ahead  were  tried.  Results  are  presented 
in  Table  5.3.  Computation  time  for  one  loop  of  sequential  search  is 
shown  in  Table  5.4.  Details  of  the  algorithms  used  are  in  Appendix  III. 

In  general,  the  results  are  encouraging.  It  seems  that  this 
approach  works  better  in  three-dimension  than  in  two  as  the  errors 
sre  lower.  The  reason  for  the  improved  behavior  can  be  attributed  to 
the  additional  possible  states  at  each  joint.  Computation  time  is 
longer  in  three  dimensions,  limiting  look-ahead  to  one  stage  if  real 
time  problems  are  to  be  undertaken  by  the  arm. 

5.4  Discussion 

Many  variations  of  the  aforementioned  strategies  are  possible. 

For  example,  one  may  start  sequential  searching  and  making  moves  at 
the  hand  then  work  toward  the  origin.  It  is  also  possible  to  find 
the  joint  at  which  a  change  would  reduce  the  error  by  the  largest 
amount,  make  this  change,  and  then  continue  the  process  of  changing 
the  joint  that  makes  maximum  reduction  in  error. 

A  reason  for  starting  at  the  origin  and  working  toward  the  hand 
is  that  in  general  a  fixed  rotation  near  the  origin  will  cause  the 
greatest  deflection  of  the  hand.  Thus,  in  many  cases  making  a  change 
near  the  origin  turns  out  to  be  the  change  that  makes  the  maximum 
reduction  in  error. 

Another  approach  is  to  take  two  joints  at  random  and  consider 
the  result  of  simultaneous  changes  in  each.  The  two-dimensional 


-88- 


! 


n 

o 

n 

o 

o 

H 

0 

n 

n 

n 

ii 

a 

D 

c 

o 

n 

ii 

0 


GE  LOOK- 
BY  1 

ERROR 

.  2 

in 

O' 

o 

3 

0 

5 

r 

6 

2  188 

1-STA 

.LOWED 

NONE 

Z 

a 

op 

K 

6 

t 

$ 

fi 

8 

R 

W  O  H  Z  W 

O  W  W  O  X 

3  m  a  h 

Cm  Q  H  < 

Q  <  CL  M  UJ 

r* 

r- 

(O 

fS 

cr 

in 

Si 

? 

0 

3  2  g  g  os 
<  W  Ou  X 

H 

a 

N 

cr 

O' 

fi 

§ 

Arm. 

mOOK- 
D  BY  1 

ERROR 

in2 

6 

(0 

r- 

0 

in 

' 

® 

m 

-•  wo 

m  o  5  < 

«  Z  Q  H  M 

•>-i  w  w 

g 

O 

i 

l 

$ 

oo  u.  w  > 

w  o  O  — <  O 

a  w  z  w  >« 

0u  w  o  S 

*->  o  a  o  w  u 

* 

CP 

cr 

l 

m  o  ■<  h  < 

C  WWW  M  UJ 

0  g  O  «  S' 

->  <  5  O  X 

U)  W  Ou 

c 

8 

oi 

£ 

O' 

5 

r: 

§ 

0J  - - - nr 

6  X  O  w  fc 

5  a 

M 

n 

<0 

to 

to  a  3- 

AD.  SEA 

L 

D 

ERROR 

in2 

O 

6 

<r 

0 

o 

r* 

8 

b 

S 

b 

w  H  U1 

TJ  X  H  O 

V  <  z  z 

W  I  X  <;  M 

o.  ©  Q  U 

in 

or 

r 

? 

0 

f- 

IN 

« 

55 

a 

a  o  w 

<fl  w  x  o  a 

z  z  z  w  >< 

x  CD  l-u  M  O  X 

O  U  H  X  M  u 

* 

K> 

£ 

8 

c 

2 

o 

oO  H  O  O  M  W 

O  Vi  (J  Z  V3  X 

*->  1  OX 

<fl  — 1  Ou 

Im 

8 

c* 

5 

g 

a 

<r 

tA  o  u*  £ 

2  o  5 

<o 

*> 

i- 

(0 

3  U 

O  X  X 

•rU  <  O 

L-  W  X  <N 

<«  x  w  ax  c 

>  t-i  ui  W  — 4 

Jo 

0 

vfl 

8 

6 

0 

H 

$ 

6 

E  •  Z  Z 

o  a  x  < 

u,  <  X  N 

u-  W  Q  <J 

« 

9 

8 

? 

5 

e 

8 

S 

X  <  X  O  Z  Q 

<M  i  z  z  o  w 

— >  x  w  w  I-H  X  H 

3  OH”  HO 

? 

«n 

8 

o 

3 

rj 

rx 

£ 

'X  O  Z  H  *-*  <!— 

41  W  O  O  V  W 

X  U  z  C  X  X 

O  Ou 

z 

fO 

s® 

£ 

cr 

r- 

Table  5.3 

TARGFT 

(in.) 

X.Y.Z _ 

CO 

04* 

0 

0 

o 

c' 

m" 

£ 

c«' 

-89- 


TIME  PER  LOOP  (sec. 


curve  matching  scheme  could  be  extended  to  three-dimen3ions  to  give  the 
arm  a  better  starting  configuration.  It  might  be  possible  to  break  the 
spatial  problems  down  into  planar  problems.  Another  idea  is  to  divide 
space  up  into  several  regions,  store  a  configuration  that  places  the 
hand  in  each  and  then  initially  start  the  hand  in  the  region  closest 
to  the  target. 

Many  strategies  are  possible,  none  clearly  better  than  others. 
Perhaps  further  study  would  show  that  certain  ones  work  better  in  certain 
areas  of  space.  It  then  might  be  possible  for  the  computer  to  learn 
which  was  best  for  a  particular  region. 

Different  error  criteria  might  be  better.  Cartesian  coordinates, 
with  the  base  of  the  arm  aligned  along  the  x-axis  were  used.  This  might 
mean  that  error  in  the  x-direction  should  be  weighted  differently  than 
error  in  the  y-  or  z-direction.  Perhaps  the  arm  can  better  reduce  angu¬ 
lar  error  than  radial  error  and  this  should  be  taken  into  account.  Again 
learning  might  be  applicable  in  selecting  one  error  criteria  for  a  given 
region  or  for  optimizing  weights  placed  on  different  quantities  in  an 
error  function  which  is  to  be  minimized. 

In  order  that  this  arm  be  useful,  the  points  in  the  reachable  space 

24 

must  be  close  together.  With  a  24  link  arm,  there  are  8  (approximately 
2 1 

10  )  possible  configurations.  There  will  be  fewer  reachable  points  than 

24 

configurations,  but  reducing  8  by  a  factor  of  10  or  100  or  1000 

still  leaves  a  large  number  of  points.  Near  the  boundary  of  reachable 

space,  the  points  will  be  further  apart,  but  in  the  interior,  the 

density  whould  be  very  high.  Assuming  that  a  24  link  arm  has  a  working 
4  18 

volume  of  5x10  cubic  inches  and  10  reachable  points  exist,  then 
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the  average  density  is  2x10  points  per  cubic  inch,  or  if  the  points 
were  equally  spaced,  they  could  be  0.00004  inches  apart.  This  leads 
one  to  believe  reachable  points  should  be  close  enough  to  any  arbitrary 
point  in  the  space.  The  problem,  of  course,  is  to  find  the  configura¬ 
tion  that  leads  to  a  position  near  a  desired  point. 

The  results  indicate  that  it  is  possible  to  find  a  solution  for 
this  digital  manipulator.  The  solutions  obtained  are  far  from  optimal 
but  close  enough  to  be  useful.  The  dimensionality  of  the  problem  is 
staggering  at  times,  but  it  is  in  fact  the  large  number  of  solutions 
that  give  hope  for  any  sub-optimal  technique. 

Further  improvement  is  possible.  Bv  streamlining  the  subroutines 
used  for  basic  computations,  computer  time  could  be  reduced.  The 
incorporation  of  different  strategies  for  different  zones  would  be 
useful. 

Although  the  problem  of  finding  a  set  of  angles  to  place  the  hand 
at  a  given  target  appears  soluble,  the  arm  itself  has  serious  limitations. 
The  primary  drawback  is  the  inability  to  control  its  motion.  Since 
there  are  discrete  states  at  each  joint,  a  wild  motion  is  likely  as 
each  change  is  made.  That  is,  the  position  is  undefined  when  motion 
occurs.  In  addition,  positions  close  in  spac  may  be  very  different  in 
arm  configuration.  In  conclusion,  the  arm  is  interesting  but  in  its 
present  state  has  no  immediate  usefulness. 
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CHAPTER  VI 

TRAJECTORY  GENERATION 


6.1  Problem  Statement 

In  remote  manipulation  a  typical  problem  is  to  move  from  an  initial 
configuration  to  some  new  position  and  then  to  grasp  an  object.  In 
order  to  carry  out  this  task,  the  position  problem  must  be  solved.  This 
results  in  a  set  of  values  specifying  how  much  to  rotate  each  joint  in 
order  to  move  the  manipulator  from  its  current  or  initial  configuration 
;o  the  desired  final  state.  However,  in  such  a  case  no  explicit  infor¬ 
mation  exists  describing  the  intermediate  states  between  the  initial  and 
final  position.  It  should  be  noted  that  the  initial  and  the  final 
configurations  may  be  physically  far  apart,  and  the  s,  ace  through  which 
the  manipulator  must  move  to  attain  the  final  state  will  in  general 
contain  obstacles.  It  is  therefore  necessary  to  find  a  "path"  along 
which  the  manipulator  can  move  nnd  not  collide  with  any  of  the  obstacles. 
This  problem  will  be  referred  to  as  trajectory  generation.  We  attempt 
to  solve  this  by  defining  sets  of  intermediate  values  for  the  joint 
angles  which  lead  the  manipulator  to  the  final  state  in  a  manner  which 
avoids  collisions. 

A  person  performing  manipulative  tasks  avoids  obstacles  very  simply. 
His  eyes  observe  a  possible  conflict  and  he  knows  intuitively  to  raise 
his  (  bow  or  change  his  direction  slightly.  He  sees  "the  world"  in 
which  he  is  working.  He  knows  immediately  which  objects  he  is  likely  to 
encounter  and  which  he  will  not  come  near.  For  a  computer  controlled 
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manipulator  the  problem  is  not  so  simple.  The  problems  of  "world" 
modeling,  conflict  detection,  and  collision  avoidance  must  all  be  fac.*d 
in  order  to  generate  a  trajectory  between  initial  and  final  manipulator 
configurations. 

As  a  first  step  in  dealing  with  this  very  difficult  problem,  a  set 
of  routines  have  been  developed  that  provide  a  mathematical  description 
of  the  world.  Other  routines  simulate  proposed  trajectories  through 
the  space  and  sequentially  examine  points  along  the  trajectory  for 
obstacle  conflict.  If  conflict  is  detected  these  routines  suitably 
modify  the  trajectory.  Several  basic  strategies  to  get  from  the  initial 
to  the  final  position  are  programmed  so  that  if  one  fails,  another  can 
be  explored.  A  block  diagram  of  this  syscem  is  shown  in  Figure  6.1. 

In  the  development  of  these  routines,  an  attempt  has  been  made  to 
be  as  general  as  possible  in  order  that  the  programs  be  applicable  to 
any  manipulator,  performing  a  wide  variety  of  tasks.  In  the  next 
sections,  we  present  a  description  of  these  routines. 

6.2  World  Model,  Obstacle  Description,  and  Conflict  Detection 

For  this  system  a  simple  model  of  the  "world"  is  used.  The  basic 
elements  of  the  world  are  assumed  to  be:  planes,  spheres  and  cylinders. 
It  is  assumed  that  all  objects  of  interest  can  be  modeled  with  these 
elements. 

The  boundaries  of  the  workspace,  usually  formed  by  table  tops  or 
walls,  are  modeled  as  infinite  planes.  These  planes  are  represented  by 
a  unit  vector,  b,  and  by  scalar  t.  Vector  b  is  normal  to  the  plane  and 
points  inward  toward  the  workspace.  Scalar  t,  the  distance  of  the  plane 
from  the  origin,  is  measured  in  the  b  -  direction. 
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Figure  6.1.  Block  diagram  of  system  to  generate  trajectories  through  a  space  containing  obstacles 


Simple,  somewhat  regular  objects  which  are  not  planar  or  cylindrical 
in  shape  are  modeled  as  the  smallest  sphere  that  circumscribes  the 
object.  A  typical  object  modeled  in  this  manner  might  be  a  cube,  a 
pencil  sharpener  or  a  coffee  cup.  The  assumption  is  made  that  all  such 
objects  are  supported  by  an  infinite  plane.  Thus  we  represent  a  sphere 
by  a,  a  vector  describing  the  location  of  its  center,  b,  a  unit  vector 
from  the  direction  of  support,  and  t,  the  radius  of  the  sphere. 

Cylinders  are  used  to  model  objects  containing  a  predominant  axis 
such  as  a  tower.  In  addition,  cyiinders  are  building  blocks  for  more 
complex  objects.  For  example,  a  manipulator  is  modeled  as  a  group  of 
cylinders  each  of  which  corresponds  to  one  of  the  manipulator's 
structural  members.  The  assumption  is  made  that  all  cylinders  are 
supported  from  an  infinite  plane  or  from  another  cylinder.  We  then 
represent  a  cylinder  by  a  line  segment  corresponding  to  its  axis  and  by 
the  maximum  distance  of  points  in  the  object  from  this  line,  d.  We  have 
then: 

b:  a  unit  vector  parallel  to  the  axis  pointing  away 
from  the  direction  of  support 
a:  a  vector  describing  the  position  of  the  base  of 
the  axis 

t:  the  length  of  the  axis 

d:  the  radius  of  the  cylinder 

With  this  representation  it  is  convenient  to  consider  the  cylinders  to 
have  a  hemisphere  capped  on  each  end  thereby  assuring  that  all  points  on 
the  surface  have  the  same  minimum  distance,  d  from  the  line  segment 
representing  the  axis. 
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Each  obstacle,  then,  has  a  list  of  properties  which  include  its 
type  (plane,  sphere,  or  cylinder),  the  composite  object  to  which  it 
belongs,  and  the  aforementioned  parameters  which  are  required  for  its 
quantitative  representation.  The  interpretation  of  a,  b,  t  and  d  is 
varied  according  to  the  type  of  the  object.  Such  models  of  all  objects 
considered  to  be  obstacles  to  the  manipulator  are  stored  in  the  computer. 

The  process  of  conflict  detection  consists  of  determining  if  the 
manipulator  and  the  objects  in  its  workspace  will  be  brought  to  the  same 
place  at  the  same  time.  This  is  accomplished  by  computing  the  distance 
between  the  elements  of  the  manipulator  and  the  elements  of  the  work¬ 
space  at  various  positions  along  the  proposed  trajectory.  A  conflict 
is  then  predicted  if  this  distance  becomes  too  small. 

It  is  undesirable  to  compute  the  distance  between  the  manipulator 
and  all  of  the  objects  in  the  workspace.  To  consider  all  the  objects  at 
each  position  along  the  trajectory  would  be  time  consuming.  In  addition, 
much  of  this  computation  would  be  wasted  as  for  an  arbitrary  position, 
the  manipulator  would  be  so  far  from  a  large  number  of  objects  that  a 
collision  with  these  would  be  very  unlikely.  We  would  thus  like  to 
consider  only  objects  near  the  manipulator.  For  this  reason  we  divide 
the  reachable  space  of  the  manipulator  into  small  regions.  In  all  the 
work  to  date,  sixty-four  subdivisions  have  been  used.  The  workspace  is 
considered  to  be  a  rectangular  para  lie lapiped  with  edges  parallel  to 
the  axes  of  a  fixed  Cartesian  coordinate  system.  The  small  regions  are 
defined  as  the  volumes  between  three  sets  of  equally  spaced  planes  parallel 
to  each  of  three  mutually  orthogonal  faces  of  the  workspace.  Then  a  list 
of  objects  completely  or  partially  inside  each  region  is  associated  with 
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that  region.  For  conflict  detection,  only  the  objects  occupying  the  same 
region  or  regions  as  the  manipulator  are  considered. 

As  a  result  ot  dividing  the  space  into  regions,  we  have  the  problem 
of  finding  in  which  region(s)  various  obstacles  are  located.  In  addition 
we  will  have  to  identify  the  region(s)  the  manipulator  occupies  at  various 
positions  along  its  trajectory.  We  wish  to  keep  this  analysis  simple  in 
order  that  the  time  saved  in  not  having  to  deal  with  all  obstacles  in 
the  workspace  is  not  lost  in  trying  to  locate  the  manipulator  in  various 
regions.  Since  the  faces  of  our  subdivisions  are  made  perpendicular  to 
the  coordinate  axes,  we  can  easily  eliminate  many  regions  by  comparing 
the  minimum  and  maximum  coordinates  (x,  y,  z)  of  an  obstacle,  with  the 
coordinate  boundaries  of  the  regions.  To  find  in  which  of  the  remaining 
regions  an  obstacle  lies  we  compute  the  distance  from  the  center  of  each 
of  the  regions  to  the  obstacle  (a  fairly  simple  process  in  view  of  the 
simple  world  model).  We  then c ompare  this  distance  with  the  radius  of 
a  sphere  totally  enclosing  the  region  to  determine  if  the  object  is  in 
the  sphere.  If  the  object  is  in  the  sphere,  we  assume  it  to  be  in  the 
region.  This  procedure  may  cause  an  object  to  be  considered  inside  a 
region  when  in  reality  it  is  outside.  However,  this  process  is  con¬ 
siderably  simpler  than  trying  to  find  whether  an  object  cuts  any  part 
of  the  actual  region. 

Routines  were  developed  which  divide  space  into  regions  and 
appropriately  enter  or  remove  objects  from  lists  associated  with  the 
regions.  These  routines  also  store  the  properties  of  each  obstacle. 

The  conflict  detection  routine  starts  with  the  first  link  of  the 
manipulator  and  finds  which  regions  this  link  is  in.  It  then  finds  the 
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distances  between  all  the  objects  in  these  regions  and  the  link.  A 
collision  is  predicted  if  the  distance  between  any  object  and  the  link 
is  small  enough  so  that,  with  the  link  continuing  along  its  present 
course,  a  conflict  would  occur.  If  a  collision  is  predicted,  a  flag  is 
set  and  the  routine  specifics  the  obstacle  and  the  link  closest  to  the 
obstacle.  It  no  collision  is  detected  the  procedure  is  carried  out  for 
the  remaining  links  in  the  manipulator.  A  block  diagram  of  this  program 
is  presented  in  Figure  6.2. 

The  method  for  determining  distance  between  a  manipulator  link  and 
an  object  depends  upon  the  type  of  object.  For  spherical  objects,  the 
distance  between  the  sphere  center  and  the  cylinder-axis  of  the  link  is 
computed.  The  actual  distance  is  then  found  by  decreasing  this  by  the 
sum  of  the  radii  of  the  sphere  and  the  cylinder  (representing  the  link). 

For  planes,  the  distance  between  the  plane  and  the  cylinder-axis  of  the 
link  is  computed.  This  distance  is  decreased  by  the  radius  of  the 
cylinder  to  form  the  actual  distance.  For  objects  modeled  as  cylinders, 
we  find  the  distance  between  the  object  axis  and  link  axis,  end  decrease 
this  by  the  combined  radii  of  the  cylinders.  Details  of  these  calculations 
are  found  in  Appendix  IV. 

6.3  Trajectory  Generation  and  Obstacle  Avoidance 

We  have  the  problem  of  finding  a  series  of  closely  spaced  inter¬ 
mediate  positions  connecting  initial  and  final  states.  These  represent 
a  trajectory  that  the  manipulator  can  follow  while  avoiding  all  obstacles 
in  the  workspace.  The  approach  used  is  to  start  by  choosing  a  plausible 
trajectory,  simulate  the  motion  along  the  trajectory  and  then  if  conflict 
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Figure  6.2.  Block  diagram  for  conflict  detection  routine. 
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occurs,  to  modify  the  trajectory.  This  modification  is  made  on  the  basis 
of  local  geometric  conditions  in  the  area  of  conflict.  (A  program, 
called  AVOID,  accomplishing  this  will  be  discussed  in  more  detail  later 
in  this  section.) 

Often  if  more  than  one  obstacle  is  present,  a  move  that  appears 
good  to  avoid  one  obstacle  is  bad  to  avoid  another.  This  may  lead  the 
manipulator  to  oscillate  between  objects.  It  is  also  possible  for  some 
joints  to  be  at  their  physical  limits  so  that  the  avoidance  routine  does 
not  find  a  good  move.  Finally,  the  avoidance  routine  itself  may  come  up 
with  a  non-productive  move.  It  is  therefore  necessary  to  continually 
ascertain  whether  or  not  progress  is  being  made  toward  the  goal.  If  no 
progress  is  being  made,  it  is  then  necessary  to  decide  whether  a  slight 
change  in  strategy  is  sufficient  or  whether  a  whole  new  strategy  is  in 
order. 

A  program  based  on  the  above  approach  called  TRLTRJ,  has  been 
written.  The  inputs  to  the  program  are  two  sets  of  joint  angles,  one 
set  specifying  the  initial  position  and  the  other  specifying  the  final 
position.  In  addition  the  desired  increment  between  intermediate 
positions  is  specified.  The  output  from  the  program  is  an  array  of 
angles  specifying  the  intermediate  positions. 

Four  basic  strategies  are  built  into  the  program.  The  first,  and 
least  complex,  just  increments  each  angle  towards  the  final  goal.  The 
second  strategy  computes  two  intermediate  positions  to  move  the  manipu¬ 
lator  up  and  then  over  a  concentration  of  obstacles.  The  third  and 
fourth  strategies  both  try  to  fold  the  manipulator  to  shorten  it  and  then 
move  it  in  front  of  any  obstacles.  These  last  two  differ  in  that  one 
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shortens  the  manipulator  by  moving  joints  in  one  direction  toward  the 
physical  stops  while  the  other  folds  the  manipulator  by  moving  the  joints 
toward  the  stops  in  the  other  direction  (in  the  program,  we  call  these 
directions  positive  and  negative,  respectively). 

The  program  starts  by  trying  the  first  strategy.  :f  any  obstacle 
is  encountered,  this  strategy  is  abandoned  for  the  time  being  and  the 
second  strategy  started.  If  while  pursuing  this  second  basic  strategy, 
a  conflict  is  predicted,  an  attempt  is  made  to  modify  the  trajectory 
using  the  program  AVOID.  If  this  strategy  fails  after  using  AVOID  the  program 
continues  and  tries  the  third  and  finally,  if  necessary,  the  "our  strategy 
in  a  similar  manner.  If  the  fourth  strategy  fails,  the  program  returns  to 
the  first  strategy  and  tries  it  using  AVOID.  If  it  does  not  produce  a 
trajectory,  it  is  assumed  that  all  obstacle  avoidance  strategies  have 
failed  and  the  program  halts. 

If  any  of  the  following  occur,  the  program  considers  that  no  progress 
is  being  made  and  hence  a  strategy  has  failed: 

1.  The  avoidance  routine  (AVOID)  is  not  able  to  generate  a 
move  due  to  joints  being  at  their  physical  stops. 

2.  A  collision  is  predicted  with  the  manipulator  at  the  same 
point  on  the  trajectory  where  a  conflict  had  previously 
been  predicted  with  that  same  obstacle  (hence  it  is 
assumed  the  program  is  in  a  loop) . 

3.  A  conflict  is  predicted  with  a  plane  for  the  second 
time,  while  trying  to  avoid  the  same  obstacle. 

(Assumedly  we  cannot  get  around  the  obstacle.) 
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4.  The  manipulator  oscillates  between  two  obstacles,  and  no 
net  progress  toward  a  goal  is  being  made. 

5.  More  than  200  intermediate  sets  of  angles  have  been 
selected  without  the  manipulator  reaching  a  goal. 

6.  More  than  350  intermediate  sets  of  angles  have  been 
explored. 

The  following  conditions  cause  a  slight  change  in  strategy  but  do 
not  cause  the  strategy  to  be  abandoned. 

1.  A  plane  of  infinite  extent  is  encountered  while  trying 
to  avoid  an  obstacle.  At  this  point  we  assume  that 
the  manipulator  if,  moving  in  the  wrong  direction  to  go 
around  this  obstacle.  The  strategy  is  to  go  back  to  the 
first  point  we  encountered  this  obstacle  and  try  to  go 
around  it  by  going  in  the  opposite  direction.  (This 
notion  of  direction  will  become  more  clear  with  the 
description  of  AVOID.) 

2.  An  oscillation  of  the  manipulator  between  two  obstacles 
is  detected.  The  action  that  the  program  takes  is  to 
go  hack  to  the  point  where  the  second  of  the  obstacles 
was  encountered  and  try  to  go  around  it  in  the  opposite 
direction.  If  this  happens  twice  at  the  same  position 
oi  the  trajectory,  it  is  assumed  that  no  progress  is 
being  made  and  the  strategy  has  failed. 

A  block  diagram  of  TRLTRJ  is  presented  in  Figure  6.3. 

The  subroutine  AVOID  is  used  to  generate  small  perterbations  in  a 
trajectory  when  conflict  is  predicted.  The  program  attempts  to  define 
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Figure  6.3.  Block  diagram  of  T^LTRJ. 


a  "good"  direction  ar.d  a  "bad"  direction.  It  then  tries  to  move  the 
link  for  which  a  collision  has  been  predicted  as  much  as  possible  in  the 
good  direction  while  not  moving  in  the  bad  direction.  This  is  accomplished 
by  defining  small  changes  in  the  joint  angles  of  all  the  links  between  the 
base  and  the  "colliding"  link.  Ideally  these  angle  changes  are  chosen  so 
that  the  link  will  have  a  large  velocity  component  in  the  good  direction 
and  zero  component  in  the  bad  direction.  If  the  link  does  not  have  enough 
freedom  (i.e.,  there  are  too  few  joints  preceding  the  link  or  the  joints 
are  at  their  physical  stops)  to  make  a  move  in  this  manner,  an  attempt 
is  made  to  move  in  the  negative  bad-direction.  If  this  too  is  not 
possible  then  no  move  is  made  and  a  flag  is  set  Indicating  that  the 
strategy  has  failed. 

The  underlying  idea  used  in  choosing  a  good  direction  is  that  all 
obstacles  are  supported  by  either  an  infinite  plane  or  another  obstacle. 
Then  if  an  obstacle  lies  between  the  manipulator  and  the  target,  one 
could  eventually  get  around  the  obstacle  by  moving  away  from  the  direction 
of  support.  In  addition  an  attempt  is  made  to  move  in  the  general 
direction  of  the  target.  This  target  will  normally  be  the  final  position 
but  may  be  an  intermediate  goal  generated  in  a  strategy  of  TRLTRJ.  If 
the  predicted< conflict  has  occurred  in  the  process  of  avoiding  a  different 
obstacle,  the  target  becomes  the  position  generated  by  AVOID  when  the 
manipulator  encountered  the  first  obstacle.  The  good  direction  is  chosen 
taking  into  account  the  type  of  obsta  'e  and  the  relation  between  link, 
obstacle  and  targe*-  as  follows: 

If  the  obstacle  is  a  plane  or  a  sphere,  the  good  direction  is 
specified  by  a  vector  from  the  point  of  conflict  on  the  link  to  the  same 
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point  on  the  link  with  the  manipulator  at  the  target  position.  (The 
assumption  here  is  that  the  sphere  is  relatively  small  and  lies  on  the 
plane  of  support.) 

If  the  obstacle  is  a  cylinder,  then  the  process  is  a  bit  more 
complicated.  Retail  that  a  cylinder  may  be  part  of  a  more  complex 
obstacle  (for  example  the  towers  in  Figure  6.4).  If  no  other  part  of 
the  possibly  complex  obstacle,  of  which  the  cylindrical  obstacle  is  a 
member  has  been  recently  encountered,  then  the  good  direction  is: 

1.  The  direction  of  the  axis  of  cylinder,  if  the 
obstacle  appears*  to  be  between  the  manipulator  and 
the  target. 

2.  The  vector  sum  of  unit  vectors  in  the  axis  direction 
and  the  direction  the  link  must  move  to  get  to  the 
target,  if  the  'ink  is  above  the  cylinder. 

3.  The  direction  the  link  must  move  to  get  to  the 
target,  if  obstacle  is  not  between  the  manipulator 
and  the  target. 

When  the  cylindrical  obstacle  is  part  of  a  more  complex  obstacle 
and  when  an  element  of  this  complex  obstacle  has  been  previously  en¬ 
countered,  then  the  good  direction  is  similar  to  the  above  with  the 
following  exception:  the  positive  axis  direction  is  replaced  by  the 
negative  axis  direction  whenever  the  point  of  conflict  on  the  obstacle 
is  nearer  the  far  end  of  the  obstacle  (i.e.,  away  from  the  point  of 

*We  say  "appears"  because  a  cylindrical  object  may  not  itself  be  between 
the  manipulator  and  the  goal,  but  the  complex  obstacle  which  it  belongs 
to,  may  indeed  be  between  the  manipulator  and  the  goal. 
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(a)  simple  tower 


(b)  Y-shaped  tower 


Figure  6.4.  Towers  used  as  obstacles.  (a)  racdel  is  a  single  cylinder 
(b)  and  (c)  are  each  modeled  with  three  cylinders. 


support).  In  this  way  we  are  able  to  follow  the  contour  of  a  complex 
obstacle. 

Once  a  good  direction  has  been  established  for  an  obstacle  it 
remains  the  same  until: 

1.  The  manipulator  is  clear  of  the  obstacles,  or 

2.  An  oscillation  has  been  detected jby  TRLTRJ,  in 
which  case  wherever  the  positive  axis  direction 

was  to  be  used,  it  is  to  be  replaced  by  the  negative 
axis  direction  and  vice-versa. 

The  bad  direction  is  always  specified  by  a  vector  from  the  link  to 
the  obstacle,  along  the  line  defining  the  minimum  distance  between  them. 

A  block  diagram  of  AVOID  is  presented  in  Figure  6.5. 

6.4  A  Test  of  the  Program 

The  trajectory  generating  routines  were  tested  by  incorporating 
them  into  the  block  stacking  program  developed  at  the  Stanford 
Artificial  Intelligence  Project  [25 J.  The  block  stacking  program 
represents  current  research  work  in  hand-eye  systems.  An  electric 
motor  driven  manipulator  of  type  6R,  s^  a^  (see  Figure  6.6)  and 

a  vidicon  T.V.,  interfaced  with  the  PDP-6  computer  form  the  basic  system. 
Programs  written  by  Singer  and  Pingle  [25 J  enable  the  manipulator  to  pick 
up  blocks  from  a  table  and  build  block  towers.  The  blocks  are  originally 
placed  at  random  on  the  table  but  within  view  of  the  vidicon.  A  block 
is  then  located  on  the  table  by  appropriate  analysis  of  the  T.V.  picture. 
Next,  the  manipulator  moves  to  grasp  the  block  and  then  places  it  to 
build  a  tower.  A  new  block  is  found  and  the  process  continues. 
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Figure  6.5.  Block  diagram  of  AVOID. 
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Figure  6.6.  Electric  Arm  at  Stanford  Artificial  Intelligence  Project. 

A  prothe?  tic  arm  originally  built  at  Rancho  Los  Amigos 
Hospital,  this  arm  has  been  modified  for  use  in  hand-eye 
research . 
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The  trajectory  generating  routine  is  used  to  find  trajectories  when¬ 
ever  the  arm  is  moved  except  in  the  last  stages  of  actually  picking  up 
or  setting  down  a  block.  At  these  times,  the  manipulator  control  is 
transferred  to  a  special  routine  whose  function  is  to  lower  or  raise  the 
hand  along  a  specified  path  with  a  specified  orientation. 

Objects  considered  to  be  obstacles  are  the  table  top,  the  support 
structure  for  the  arm,  and  any  block  towers  that  have  been  built.  In 
addition,  to  make  life  difficult  for  the  program,  several  other  obstacles 
(see  Figure  6.4)  were  added.  Since  the  range  of  vision  of  the  T.V. 
camera  is  small,  and  its  recognition  powers  to  date  is  limited  to  cubes, 
a  sub-program  was  written  so  that  the  external  obstacles  could  be  added 
to  the  data  structure  by  commands  from  the  teletype. 

After  allowing  the  program  to  run,  with  the  different  obstacles  in 
varying  locations,  the  trajectory  generation  program  was  seen  to  perform 
fairly  well  (see  Figure  6.7).  Where  possible,  it  was  generally  able  to 
go  over  or  in  front  of  the  obstacles.  However,  the  procedure  occasionally 
failed  when  the  manipulator  wr.s  started  in  a  configuration  in  which  joints 
were  near  their  physical  steps.  In  these  cases  a  successful  maneuver 
might  have  been  to  move  those  joints  well  away  from  the  stops  and  try 
again  (a  procedure  not  built  into  the  program).  In  addition,  if  the 
objects  were  so  placed  that  the  arm  could  only  get  through  by  going 
between  two  objects,  failure  generally  occurred. 

Whenever  more  than  one  or  two  strategies  were  tried,  the  computation 
might  run  upwards  of  20  seconds.  However,  most  manipulative  scenes  are 
fairly  static,  so  that  once  a  trajectory  had  been  found  through  a  given 
set  of  obstacles,  it  could  be  used  repeatedly.  This  process  would  save 
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having  to  re-analyze  the  trajectory  for  every  move,  thus  conserving 
computer  time.  In  addition  we  could  move  backward  on  the  same  trajectory 
to  get  back  through  the  space  again. 

With  this  program  we  have  attacked  the  problem  of  moving  a  multi¬ 
link  manipulator  through  a  space  composed  of  three-dimensional  objects. 
Had  we  been  concerned  with  having  just  the  hand  avoid  obstacles  on  a 
plane,  the  problem  would  have  been  much  less  complex,  as  the  hand  could 
be  made  to  follow  an  arbitrary  curve.  Such  is  not  the  case,  when 
considering  a  conflict  with  all  links  of  a  manipulator.  We  cannot  inde¬ 
pendently  specify  the  position  of  each  link  of  a  six  degree-of-freedom 
manipulator.  There  are  just  not  enough  freedoms.  However,  the  programs 
developed  above  do  enable  us  to  deal  with  the  problem  of  conflict  for  a 
general  multi-link  manipulator.  These  programs  perform  the  basic 
function  of  allowing  a  manipulator  to  perform  tasks  in  the  presence  of 
obstacles. 
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CHAPTER  VII 


CRITERIA  IN  THE  DESIGN 
OF  A  MANIPULATOR  FOR  COMPUTER  CONTROL 

7.1  Kinematic  Criteria 

As  mentioned  earlier,  a  manipulator  needs  to  have  six  degrees-of- 
freedom  to  grasp  a  rigid  body  with  a  specified  orientation  at  a  specified 
position  in  space.  In  addition,  the  kinematic  solution  must  be  easily 
programmed  and  solved.  This  indicates  the  desirability  of  a  closed-form 
solution  rather  tha->  iterative  techniques.  The  closed-form  solutions 
are  faster  and  find  all  configurations  leading  to  the  desired  terminal 
position  and  orientation  while  iterative  techniques  find  only  one.* 

In  fact,  the  iterative  schemes  may  not  find  a  solution  even  though 
several  may  exist.  The  question  of  the  existence  of  a  solution  is 
important,  as  this  existence  indicates  whether  a  given  position  and 
orientation  is  physically  attainable.  It  is  desirable  to  have  solutions 
exist  throughout  th>_  workspace  or  at  least  know  where  they  do  not  exist. 
Thus  a  factor  in  the  design  of  a  manipulator  is  the  zones  in  which  the 
terminal  device  can  be  placed  in  an  arbitrary  manner. 

The  ,/roblem  of  zones  is  closely  allied  to  that  of  solutions.  The 
existence  of  a  solution  for  a  given  position  and  orientation  automatically 
guarantees  that  that  point  is  within  the  zone  of  reachable  points.  One 
method  of  investigating  zones  would  be  to  solve  the  position  problem  for 

*The  iterative  technique  may  however  be  used  to  good  advantage  when  the 
distance  between  positions  is  very  small.  Then  the  iterations  converge 
quickly,  and  there  is  only  one  solution  being  sought. 
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many  points  and  many  manipulator  configurations.  This  however  is  very 
lengthy  and  not  at  all  general.  Alternatively,  we  attempt  to  give  a  few 
general  remarks  about  zones. 

When  on  the  boundary  of  the  zone  of  reachable  space,  the  hand 
cannot  be  moved  in  an  arbitrary  direction  or  rotated  about  an  arbitrary 
axis.  Another  way  of  saying  this  is  that  the  hand  cannot  move  along  an 
arbitrary  screw.  Mathematically  this  happens'  whenever  the  determinant 
formed  from  the  left-hand  sides  of  equations  (4.13)  and  (4.14)  vanishes. 

The  existence  of  a  solution  would  enable  us  to  express  the  W^  and  r^  , 
appearing  in  (4.13)  and  (4.14),  in  terms  of  the  hand  position  and 
orientation.  Then  forming  the  determinant  we  would  have  a  polynomial  in 
terms  of  the  hand  position  and  orientation  whose  vanishing  would  correspond 
to  the  boundary  of  reachable  space.  We  would  then  have  a  surface  in 
six-space  which  bounds  reachable  space. 

As  this  representation  is  highly  non-linear,  as  well  as  dependent 
upon  the  existence  of  a  solution,  it  is  often  more  fruitful  to  examine 
the  problem  from  a  geometrical  viewpoint.  For  example,  consider  the 
6R,8^s^  manipulator  with  all  adjacent  pairs  of  axes  perpendicular,  as 
is  shown  in  Figure  7.1.  We  note  that  the  wrist  point,  W  ,  defined  by 
the  vector  P  ,  can  lie  anywhere  within  a  sphere  of  radius  r  about  the 
shoulder  point,  0  ,  where. 

(s34«  5)2  (7.1) 

Furthermore,  if  the  wrist  position  is  fixed,  then  the  direction  the 
hand  points,  defined  by  a$  in  Figure  7.1,  is  arbitrary.  Through 
appropriate  rotations  in  joints  3  and  6,  a^  can  be  made  to  point  in 
any  direction.  However,  the  total  orientation  of  the  hand  cannot  be 
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arbitrarily  specified  for  a  fixed  wrist  point  as  the  direction  defined 

by  in  Figure  7.1  is  limited  in  range.  We  note  that  ^  must 

always  be  perpendicular  to  ^  .  Hence,  may  lie  anywhere  in  a 

plane  perpendicular  to  ^5  .  Now  the  specification  of  the  wrist 
% 

point  does  not  fix  the  elbow  point,  A,  and  in  fact  the  triangle  QAW  may 

be  rotated  about  P  .  We  observe,  then,  that  must  lie  on  a  cone 

whose  axis  is  P  ,  with  apex  at  W  and  whose  cone  angle  is  fixed  by 

triangle  QAW.  Then  w  will  lie  in  planes  through  W  ,  perpendicular 

-0 

to  the  elements  of  this  cone.  This  defines  a  second  cone,  inside  which 
^  can  never  point.  These  cones  are  shown  in  Figure  7.2.  Referring  to 
Figure  7.2,  the  elements  of  cone  1  form  the  locus  of  while  will 

always  lie  outside  cone  2. 

If  it  is  desirable  for  the  hand  to  have  a  full  range  of  orientation 
freedom,  then  a  manipulator  whose. last  three  Joints  are  revolute  and 
whose  axes  intersect  is  appropriate.  Consider  such' a  configuration, 
shown  in  Figure  3.8.  Here  the  last  three  axes  intersect  and  provide 
maximum  orientation  freedom  for  the  hand.  In  addition  this  configuration 
has  a  wide  range  of  positions  that  the  wrist  point,  defined  by  P3  , 
can  assume.  Referring  to  Figure  3.8  we  note  that  the  wrist  point  can  be 
placed  Anywhere  inside  a  circle  normal  to  axis  2,  about  Pj  ,  whose  radius 
r  obeys  the  constraint: 

(a2‘S4)2  ^  r2<  <C2+S4)2  (7*2) 

Rotation  of  the  first  joint  then  rotates  this  circular  anulus  to  generate 

a  torus  which  is  the  locus  of  points  the  wrist  can  reach. 

It  is  possible  to  examine  many  manipulator  configurations  in  this 
manner.  Table  7.1  presents  the  results  of  such  examination  of  6R  manipu¬ 
lators  with  two  and  three  non-zero  link  parameters.  Whether  or  not  a 
solution  exists  is  also  included  in  Table  7.1. 


-115- 


TABLE  7.1 
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7.2  Additional  Considerations 


Aside  from  kinematic  considerations,  there  are  many  less  objective 
criteria  in  choosing  a  manipulator  for  use  with  a  computer  system.  For 
the  lake  of  completeness  we  mention  some  of  these  additional  considerations, 
and  give  a  few  remarks  about  several  of  the  more  Important  ones. 

1.  Ease  of  Interface  with  a  Digital  Computer. 

The  actuators  of  a  manipulator  must  be  such  that  their 
control  may  be  easily  assumed  by  a  digital  computer. 

In  addition  position  feedback  must  be  available. 

This  will  generally  be  from  potentiometer  or  shaft 
encoders. 

2.  Power  Source. 

Manipulators  are  in  general  electrically,  hydraulically,  or 
pneumatically  powered.  Electricity  is  universally  available 
and  inexpensive.  Hydraulic  power  provides  the  means  for 
converting  a  large  amount  of  energy  to  motion  with  a 
minimum  of  weight,  thus  an  advantage  where  speed  is 
required.  Pneumatically  powered  manipulators,  working 
off  of  air,  are  cleaner  than  hydraulic  systems.  However, 
for  safety  reasons,  they  must  operate  at  a  much  lower 
pressure  and  therefore  will  have  poorer  dynamic  response. 

3.  Structural  Rigidity. 

The  structural  members  must  have  a  minimum  deformation 
under  load  so  that  the  position  of  the  hand  may  be 
accurately  computed  from  the  rotations  in  the  joints. 
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In  addition  the  joints  must  contain  a  minimum  of  play 
for  the  same  reason. 

4.  Range  of  Freedom 

It  is  desirable  that  each  joint  of  a  manipulator  possess 
a  large  range.  Even  though  a  position  might  be  reachable 
from  a  kinematic  point  of  view,  the  physical  limits  on 
actuators  will  greatly  reduce  the  range  of  these.  In 
fact,  many  of  the  problems  encountered  while  using  the 
obstacle  avoidance  programs  were  due  to  the  very 
restricted  range  of  motion  on  the  electric  arm 
(Figure  6.6) . 

5.  The  Outline  of  the  Manipulator. 

Wt  would  like  the  manipulator  to  have  a  slim  outline 
so  that  it  could  work  in  tight  places.  In  addition 
a  smooth  profile  might  be  desirable  so  that  it  would 
be  easily  recognizable  in  a  T.V.  image. 

6.  Other  Factors. 

Additional  factors  to  be  considered  are:  precision, 
speed,  cost  controllability  (i.e.,  the  ability  to 
follow  a  prescribed  path),  and  safety. 

When  choosing  a  manipulator  we  cannot  hope  to  maximize  all  of  these 
considerations.  Many  of  these  are  influenced  by  the  type  of  task  per¬ 
formed  by  the  manipulator.  For  example,  if  a  goal  for  the  hand-eye  system 
is  to  assemble  a  machine  containing  small  electronic  components,  the 
manipulator  must  be  capable  of  very  delicate  movement  and  position 
accuracy.  For  tasks  involving  throwing  or  catching  objects,  the  arm 
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must  be  able  Co  move  rapidly,  and  be  accurately  controlled.  Thua  some 
applications  require  obvious  tradeoffs  (e.g.,  precision  and  speed), 
and  In  others  certain  considerations  predominate. 

From  experience  with  the  two  manipulators  used  at  the  Stanford 
Artificial  Intelligence  Project  we  may  make  some  comment  on  specific 
arms.  The  project  presently  has  two  arms.  One  Is  a  modified  electric 
prosthetic  arm  (Figure  6.6).  The  other  Is  hydraulically  powered 
(Figure  7.3). 

The  d.c.  electric  motor  driven  arm  has  proven  acceptable  for 
stacking  blocks.  After  some  experimentation,  a  rate  modulated  pulse 
dc  system  seems  to  be  an  excellent  way  to  contro]  the  arm.  With  position 
feedback  via  potentiometers,  and  an  external  poter  supply,  it  is 
satisfactorily  interfaced  with  the  computer.  '.owever,  it  is  somewhat 
lacking  in  the  range  of  freedom  and  structural  integrity  *  problems  that 
could  be  overcome  with  a  second  generation  arm  of  this  type.  It  is  not 
particularly  fast  nor  particularly  precise.  The  precision  problem  stems 
partly  from  the  poor  structure,  and  partly  from  the  control  problem 
caused  by  the  inherent  inertia  in  the  motors.  It  is  expected  that  with 
refinement  of  the  control  scheme,  the  precision  and  controllability  could 
be  considerably  improved. 

Although  experience  with  the  hydraulic  arm  is  limited  at  this  time, 
it  shows  promise  of  great  speed.  It  also  appears  structurally  sound, 
and  has  a  wide  range  of  freedom  in  its  Joints.  It  is  somewhat  massive 
due  to  its  high  speed  and  torque  capabilities.  At  this  time,  the  control 
problem  using  two-stage  servo-valves  appears  soluble.  The  physical  danger 
to  personnel  and  equipment  is  obvious  and  this  arm  is  housed  in  a  room 
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isolated  from  the  computer.  This  makes  the  interface  with  the  computer 
T.V.  system  difficult,  though  soluble.  In  addition  the  forces  involved 
require  that  the  arm  be  firmly  anchored  to  the  floor. 
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At  present  these  manipulators  are  used  for  fairly  simple  tasks. 

As  the  hand-eye  program  becomes  more  advanced  the  tasks  will  become 
more  involved.  At  some  future  time,  then,  one  might  expect  to  be  able 

to  say  more  about  choosing  a  manipulator  for  computer  control  in  a  more  M 
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CHAPTER  VIII 


CONCLUSIONS  AND  SUGGESTIONS  FOR  FUTURE  WORK 

In  this  dissertation,  the  kinematic  problems  associated  with 
manipulators  have  been  explored.  It  is  hoped  that  the  classification 
scheme  and  catalog  of  manipulators,  presented  in  Chapter  II,  will  lead 
to  manipulators  being  compared  on  a  scientific  basis.  Manipulators  whose 
exteriors  seem  much  different,  are  often  kinematically  equivalent.  Thus 
solutions  for  one  manipulator  are  applicable  to  another. 

It  is  seen  that  the  problem  of  positioning  a  manipulator  is  directly 
related  to  the  displacement  analysis  of  mechanisms.  The  solutions 
presented  for  cases  with  three  revolute  axes  intersecting  at  a  point 
seem  to  be  previously  unknown.  These  results  therefore  represent  a 
contribution  to  spatial  linkage  analysis. 

It  is  felt  that  these  solutions,  along  with  the  extension  to  the 
special  cases  with  only  pairs  of  axes  intersecting,  give  insight  into 
the  kinematic  analysis  problem  for  the  most  general  six  degree-of- 
freedom  manipulator.  That  is,  for  the  special  case  of  three  intersecting 
pairs  of  axes,  four  different  configurations  were  found  leading  to  the 
same  hand  position  and  orientation.  For  two  pairs  of  intersecting  axes, 
eight  configurations  were  found,  and  for  only  one  pair  of  axes  inter¬ 
secting,  sixteen  configurations  were  shown  to  be  possible.  In  all  of 
these  special  cases,  adjacent  axes  were  orthogonal,  and  the  adjacent 
common  normals  intersected  one  another.  Since  no  axes  nor  adjacent 
common  normals  intersect  in  the  most  general  problem,  it  is  almost 
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certain  that  the  general  problem  has  even  more  possible  configurations 
leading  to  the  same  hand  position  and  orientation. 

The  possibility  of  ,a  very  large  number  of  configurations  indicates 
that,  even  if  a  solution  to  the  general  problem  could  be  expressed  as  a 
single  polynomial  in  one  unknown,  this  polynomial  would  be  of  such  a  high 
degree  that  it  would  be  impossible  to  find  all  the  roots.  We  conclude, 
then,  that  the  complete  solution  to  the  most  general  problem  is  not  at 
this  time  technically  feasible:  perhaps,  someone,  someday  will  solve  the 
problem.  Kinematiclans  have  been  trying  for  over  50  years. 

The  iterative  technique,  based  on  velocity,  was  found  to  be  superior 
to  the  Newton-Raphson  method  both  in  the  amount  of  time  taken  per  Iteration, 
and  in  the  range  of  distance  between  positions  where  convergence  occurred. 
The  iterative  technique  may  be  used  to  good  advantage  when  the  distance 
between  positions  is  small.  Thus  an  approximate  model  having  a  closed 
form  solution  could  be  used  to  find  starting  points  from  which  the 
numerical  procedure  could  be  used  to  find  actual  solutions. 

The  problem  of  placing  the  end  of  a  digital  manipulator  at  a  target 
appears  soluble.  The  results  have  shown  that  the  hand  can  be  placed 
close  to  an  arbitrary  point.  Different  strategies  could  be  developed 
that  might  save  computer  time  and  improve  performance.  Matching  the 
arm  to  a  curve,  as  was  done  in  the  planar  model,  would  undoubtably  help 
shape  the  arm.  However,  if  this  manipulator  is  to  be  used,  its  in¬ 
herent  drawbacks  must  be  remedied.  That  is,  the  motion  between  states 
must  be  made  controllable. 

The  trajectory  generation  and  obstacle  avoidance  routines  were 
found  to  perform  a  basic  function:  they  allow  a  manipulator  to  work 
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within  a  space  composed  of  large  obstacles.  Previous  attempts  at 
obstacle  avoidance  dealt  only  with  keeping  the  hand  away  from  obstacles. 
In  this  work,  possible  conflict  between  obstacles  and  all  links  of  the 
manipulator  is  considered.  Future  work  should  attempt  to  remedy  the 
Allowing  shortcomings: 

1.  The  description  of  obstacles  needs  to  be  improved.  In 
this  work,  obstacle  properties  were  defined  using  a  data 
structure  within  the  confines  of  FORTRAN.  Many  useful 
properties,  such  as  the  relation  between  obstacles  were 
not  stored.  With  the  development  of  a  more  sophisticated 
world  model,  using  a  higher  level  programming  language, 
the  manipulator  and  obstacles  could  be  modeled  more 
precisely.  This  would  lead  to  more  accurate  conflict 
detection  and  better  information  about  which  direction 

to  move  to  get  away  from  an  obstacle. 

2.  The  problem  of  moving  between  two  closely  spaced  obstacles 
has  not  been  adequately  solved. 

3.  The  computer  time  to  generate  trajectories  may  be 
excessive.  This  is  in  part  due  to  the  attempt  to  make 
the  routines  applicable  to  a  variety  of  manipulators. 

For  example,  the  analysis  program  used  to  compute  hand 
position  and  orientation  is  applicable  to  the  most 
general  arm.  Fast  machine  language  subroutines  to  perform 
dot  products  would  decrease  machine  time. 
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4.  At  present  the  routines  do  not  benefit  from  past 

experience.  Improvement  might  result  if  previously 
generated  trajectories  were  stored  and  parts  of  them 
were  used  over  again  when  similar  situations  arose. 

The  problem  of  zones  has  not  been  fully  explored.  Although  a 
mathematical  interpretation  of  zones  is  presented,  it  is  not  totally 
^tisfactory  as  it  Honoris  upon  the  existence  r f  a  solution.  Geometrical 
methods  give  insight  into  special  cases,  however  they  have  the  dis¬ 
advantage  of  not  being  generally  applicable. 

In  this  work  six  degree-of-freedom  manipulators  were  studied 
because  it  is  necessary  to  have  six  degrees-of-freedom  to  grasp  an 
object  at  an  arbitrary  position  with  an  arbitrary  orientation.  However, 
since  manipulators  with  more  than  six  freedoms  have  not  been  studied, 
future  work  might  involve  investigating  the  use  of  additional  freedoms. 
For  example,  extra  degrees-of-freedom  would  be  useful  in  avoiding 
obstacles. 

It  is  felt  that  the  theoretical  results  of  this  investigation,  and 
the  computer  programs  developed  from  them,  yield  a  "universal"  kinematic 
analysis  and  trajectory  generator  procedure.  It  is  expected  that  the 
package  of  computer  programs  (which  will  be  further  documented  in  a 
project  memo)  can  be  applied  to  any  six  degree-of-freedom  manipulator 
with  turning  joints. 
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APPENDIX  I 


DETAILS  OF  SOLUTION  BY  NEWTON-RAPHSON  METHOD 


The  inputs  to  the  program  are: 

NBM:  the  maximum  number  of  iterations 

THM:  the  maximum  allowable  correction  in  radians 

XBM:  the  maximum  allowable  change  in  target 


-1-2-1— 2 : 


Pi  and  P 2: 


Theta: 


Two  vectors  fixed  in  the  hand  in  their  initial  and 
final  positions.  L  is  the  direction  the  hand  points 
(the  direction  of  ^x-axis)  and  .  N  is  the  direction 
of  the  sixth  revolute  axis.  The  subscript  1  refers 
to  initial,  2  to  the  final  position  . 

Vectors  specifying  the  initial  and  final  position 
respectively  of  a  point  in  the  hand. 

A  1x6  vector  giving  the  initial  joint  angles  (i.e., 

,  i=(l,...,6) 


In  addition  the  program  uses  the  following  subprograms: 

ARMCON:  Spacifies  the  parameters  of  the  arm 
HANDPO:  Analysis  program  that  computes  the  position  and 
orientation  of  all  the  links  in  the  arm,  using 
the  present  values  of  the  joint  angles. 

MATINV:  Routine  to  invert  matrices  and  solves  the  linear 
equation  A_x_=  b  . 

The  program  basically  solves  the  matrix  equation  (4.5).  The  A£0  are 
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found  from  the  analyst*  program  and  the  coefficients  of  the  6A^  are 
generated  by  successive  matrix  multiplication.  The  matrix  Aeq  is 
obtained  from  the  inputs.  It  is  of  the  form 


Aeq  " 


(L2)  (M2)  (n2)  (P2) 
0  0  0  1 


alla12a13a14 


a21a22a23a24 

a31a32a33a34 


0  0  0  1 


(Al-1) 


Since  the  rotation  portion  of  the  matr'.x  is  composed  of  nine  elements 
and  only  three  are  independent,  we  select  the  equations  formed  from 
elements  a^,  *131  and  *32  which  together  with  a24>  a34 

give  us  six  independent  equations. 


Specification  of  Intermediate  Goal 

If  the  changes  in  position  and  orientation  represented  by  the 
right-hand  side  of  equation  (4,5)  is  too  large,  (4.5)  is  not  valid 
and  an  intermediate  goal  is  necessary.  The  unit  vector  L  and  N 
respectively  rotate  through  angles  defined  by  the  arccosines  of  L^’L2 
and  N^*N2  ,  about  axes  defined  by  L^xL2  and  NjXN2  .  For  intermediate 
goals,  and  are  rotated  through  fractions  of  their  total 

rotation.  In  addition,  the  same  fraction  of  P,2’— 1  a<*ded  to  • 

A  block  diagram  of  this  is  presented. 
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APPENDIX  II 


DETAILS  OF  ITERATIVE  VELOCITY  METHOD 

We  first  present  a  method  for  finding  the  screw  given: 

and  M^:  Two  vectors  fixed  in  the  hand  in  its  initial  state. 

L2  and  The  same  two  vectors  sfter  a  change  in  position  and 

orientation. 

P^  and  P-:  Vectors  from  the  origin  of  the  1  system  to  the  same 
point  in  the  hand  before  snd  after  the  change  in 
position  and  orientation. 


The  direction  of  the  screw  axis  n  and  the  magnitude  of  the  rotation 
cp  can  be  found  from  the  following  statement  of  Euler's  theorem: 


cp  dtf'V  x 

n  tan  y  ■ -  ■  ■  ■ 

<k2"V  *  <’-2+-l) 

if  we  define 

(Ii2-ki)  * 


W 


(L2-£i>  ‘  (N^) 


then 


W 


(A2-1) 


(A2-2) 


|W| 

cp  -  2  arctan  |w|  . 

The  normal  from  the  origin  to  the  screw  axis,  r  ,  is  computed  from 


(A2-3) 

(A2-4) 


x-i 


P1+P2  +  W  x  (P2-P,>  W*(P2+P  > 


w 


(A2-5) 
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The  magnitude  o£  the  translation,  S,  is 
s  .  g_l  -  ll) 

l»l 

Lastly,  defining  the  pitch  of  the  screw  H  as 

H  =  — 

<P 


_  w  *  (P2  -  2i) 

'P  "  2  |W|  arcFan  (|w|) 

we  have  all  the  necessary  parameters  of  the  screw. 

We  next  show  that  for  infinites final  motion,  W  is  related  to  the 

angular  velocity.  We  write 


(A2-6) 

(A2-7) 

(A2-8) 


dLi 


d2L, 


U  -  Lj  +  At  +  -1  Atz  +  ... 

d  t  dt2 

V,  »-  ^  dNi  ,  d^.  2  . 

N~  *  h.  +  At  +  ~1  At  +  • . . 

“Z  dt: 

.0 


<p  m/,  + — 

'  1  dt 


dt* 

d^cp  2 

At  +  — x  At  +  . . . 
dt2 


(A2-9) 

(A2-10) 

(A2-11) 


using  the  above  in  (A2-2) 

1 

At  +  • • « 


AJ  x 

at  _ at 


'  t141  •  2*i 


(A2-12) 


+  •  •  • 


and  in  its  equivalent  from  (A2-1) 

W  -  tan  (|  At  +  . . .  )n  .  (A2-13) 

Then  equating  the  right-hand  sides  of  (A2-13)  and  (A2-12)and  taking 
the  limit  as  At  -*  0  we  get 


dL,  dN 
~l  *  -  1 
cp  n  =  dt  at 

dL.  .  N  , 
dt1  " 


(A2-14) 


which  are  equivalent  expressions  for  the  angular  velocity  of  the  hand. 
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(A2-15) 


If  we  define  the  approximate  angular  velocity,  u)  to  be 

A  CD 

ID  “  $t  - 

and  the  rotation  is  small  so  that  from  (A2-3)  we  find  n  and  from  (A2-4) 
Acp*  2  arctan  jw|  (A2-16) 

Now  the  approximate  velocity  of  a  point  in  the  hand  at  the  origin  is: 

V  =  Huj  -  m  x  r  (A2-17) 

«  -  A? _n  x  r  (42-18) 

At  At 

where  H,  A ®,  n,  and  _r  are  the  screw  parametera  formed  from  the 
change  in  hand  position  and  orientation. 

Inputs  to  the  program 

This  program  has  the  same  inputs  as  the  Newton-Raphson  program. 

In  addition  to  the  same  subprograms,  it  requires: 

SCREW  which  computes  the  screw  defined  by  1-2— 1-2 

using  equations  (A2-1)  -  (A2-7). 
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APPENDIX  III 


MATHEMATICAL  DETAILS  FOR  THE  ’'TGITAL  MANIPULATOR 


A3.1  Transformation  to  find  hand  position  given  the  angle*  (Planar 
Case) 

We  use  the  basic  link  model  described  in  Chapters  II  and  III. 

For  the  planer  case,  the  z-coordinates  and  the  angles  between  adjacent 
links  are  all  zero.  In  addition,  we  assume  that  all  the  common  normals 
are  the  same  length,  d  ,  so  that  we  may  rewrite  (3.1)  as 


c  Gi  -  s  dc  0j 
s0i  cGi  dsQ^ 
0  0  1 


(A3- 1) 


and  slmlliarly  from  (3.17)  we  may  describe  the  position  of  the  hand 

(x.y)  by: 

Ol 


r  - 

X 


y  I-  A  ....  An  0 

lIJ  L.1- 


(A3-2) 


where  n  is  the  number  of  links  in  the  arm. 


A3. 2  Transformation  to  Find  Hand  Position  Given  the  Angles  (3-Dimen- 
s tonal  Case) 

Consider  the  link  element  shown  in  Figure  A3.1.  The  link  model 
(Chapter  II)  and  transformations  (Chapter  III)  are  applicable  to 


igure  Aj  I ,  The  Basic  Element  for  a  Three-Dimensional  Digital  Atm. 
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this  manipulator.  We  note  that  each  of  the  "joints"  of  Figure  A3.1 
actually  contains  two  degrees  01  freedom.  Using  (3.1)  we  may  tfrite 
the  transformation  due  to  a  rotation  about  each  axis,  so  that  the 
transformation  between  adjacent  elements  may  be  written 


ceic”l 

-801 

-c0i  scpt 

d  c0i  ccp 

ccpi 

C0i 

-s0i  sqpt 

d  8  0£  ccp 

scpi 

0 

ccp1 

d  scpt 

0 

0 

0 

1 

—  “ 

X 

0 

y 

*  Bi  •  •  •  •  B 

1  n 

0 

z 

1 

0 

1 

(A3-3) 

and  the  coordinates,  of  the  end  point  of  the  last  link,  (x,y,z)  are 

(A3-4) 

where  n  is  the  number  of  elements  in  the  arm. 

To  find  rotations  about  axes  z^  and  z'  (Figure  A3.1)  which 
lead  to  a  tilt  of  6Q  about  an  axis  midway  between  z^  and  z '  , 
we  note  that  this  is  equivalent  to  rotating  z^  through  45°  about 
x^  and  then  rotating  x^+^  through  0Q  about  the  new  z^  axis. 

Then  using  equation  (3.1)  to  express  these  rotations,  the  resulting 
transformation  matrix  is 


T  - 


C0o 

-s0o 

0 

dcfi0 

8  0o 

c0o 

-1 

d«e0 

2 

/2 

72 

sT 2 

8  0 

C0- 

1 

ds  0 

7f 

o 

7 T 

72 

if 

0 

0 

0 

1 

(A3-5) 


Then  the  direction  of  the  x^+^  axis  is  represented  by  a  vector  com¬ 


posed  of  the  'll',  '21',  and  '31'  elements  of  (A3-5).  These  elements 
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must  be  equal  to  the  corresponding  elements  of  (A3-3)  which  lead  to: 
scp  =  s0Q 

7T 

and  1 0O 
t6“  -^2  — 

A3. 3  Derivation  of  Curve  Composed  of  Segments  of  Four  Circles 
We  want  to  find  a  curve  made  up  of  segments  of  four  circles 
connected  in  such  a  way  that  adjacent  circles  are  tangent  to  one 
another.  Thus  a  smooth  transition  between  the  elements  of  the  curve 
exists.  In  addition  we  require  that  the  total  arc  length  be  specified. 
We  also  specify  the  slope  of  the  curve  at  each  end  and  the  radii  of 
the  circles.  Consider  such  a  curve  shown  in  Figure  A3. 2.  Given  the 
radii  of  the  circles  and  the  position  of  the  base  of  the  arm,  we 
easily  locate  center  A. 
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The  known  angle  that  the  tangent  to  the  curve  at  the  base  makes  with 

g 

the  horizontal  is  one-half  the  allowable  joint  rotation.  The 

end  point  on  the  curve  is  specified  by  its  coordinates  (x,y),  and  the 

end  slope  by  the  angle  Cp^  .  This  then  fixes  center  D  ,  from  which 

the  line  segment  51  ,  and  the  angles  cp  and  a  are  defined.  Next 

we  must  locate  the  centers  B  and  C  ,  and  find  the  angles  P>2 • 

83,  and  0^  which  define  each  segment  of  the  curve.  For  the  purpose 
of  derivation,  we  introduce  the  angle  0^  between  line  ST5  and  51  . 
If  we  denote  the  radii  of  the  circles  by  r  ,  then: 

I-r  1 


IB  *  1C  =  11  ■  2r 


(A3-6) 


and  define  R  such  that 
R  *  51  . 


(A3-7) 


Then  from  Figure  A3-2  we  observe 

6l  +  0Q  -  a  +  e'  (A3-8) 

and 

cp  *  flj  +  +  (n  -  62)  "  (  71 "  63)  +  Crr  -  0^) 

or 

(0!  +  e3)  -  (02  +  e4)  -  <P  -  ^  -  n  (A3-9) 

Next  impose  the  constraint  that  the  total  arc  length  is  L  : 

*(01  +  02  +  63  +  64)  "  L 
or 

(0i  +  03>  +  (02  +  04)  -  7  #  (A3-10) 
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Combining  (A3-9)  and  (A3- 10)  by  subtraction  and  addition  yields 


(0!  +  03)  -  £  (7  +  (T-  7s  -"  ) 

(A3- 11) 

(92  +  e4)  -  £  (£  -  <p  +  +  *)  . 

(A3- 12) 

Writing  the  law  of  cosines  fox  ABCD  yields 

■gC2  =  8r2(l  -  cos03)  • 

(A3- 13) 

Then  writing  the  law  of  cosines  for  ABAD  yields 

*)  2  2 

TH52  *  4r  +  R  -4rR  cos8^ 

(A3- 14) 

2 

and  we  may  easily  eliminate  T5I5  from  (A3- 13)  and  (A3-14) 

to  obtain 

cos  fli  -  -  |  +  i  cos  03  . 

(A3-15) 

Now  we  combine  (A3-8)  and  (A3-11)  to  get 

01  +  83  “  P 

(A3- 16) 

where  L  0_ 

8  -  l(r  "  -  2a  +  7“) 

(A3-17) 

We  next  eliminate  63  between  (A3- 15)  and  (A3- 16),  which 

after 

simplification  results  in 

k^  cos2  0'  +  k^  cos  0j  +  k3  ■  0 

(A3- 18) 

where 

4r  r2 

kx  - 1  -  r cos  B  +  V 

(A3- 19) 

2r  1 

k  =  2(1  -  —  cos  p)  (|  “  4r  ) 

2  R  R  R 

(A3-20) 

r  1  2  r2  2 

k3  *  <R  "  4  f  '  (1  “  C0S  B) 

K 

(A3-21) 

from  which  may  be  obtained.  Knowing  c'  we  compute  8^  from 

(A3-8)  which  locates  center  B  .  Once  B  is  known,  center  C  is 
found  by  considering  the  intersection  of  two  circles  of  radius  2r  , 
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one  with  center  at  B  ,  the  other  at  A  .  Once  the  circle  centers 
are  located,  the  angles  »  63  >  64  can  be  easily  found. 


A3. 4  Description  of  Programs  -  2-Dimensional  Model 

First  the  arm  is  put  into  an  initial  configuration  either  in  an 
arbitrary  manner  or  with  the  subroutine  INITIAL  that  matches  the  arm  to 
the  curve  composed  of  circle  segments. 

Recalling  that  the  coordinate  transformation  exists: 


the  multiplication  of  the  transformation  is  broken  into  3  parts 

<Al  "■  Alndex  -j)  <Ai"‘1‘=*  *Aindex  +  look'  (AIndex  +  look  +  1* '  * 
where  "index"  is  the  number  of  the  joint  under  consideration  and  "look" 

is  an  integer  giving  the  number  of  stages  of  look-ahead.  Then  the 
first  and  third  term  are  generated  by  a  subroutine  that  transforms 
coordinates.  These  are  temporarily  stored.  Then  the  middle  term  is 
generated  for  all  possible  0^  ,  i  =  index  ,  ...  ,  index  +  look  .  The 
matrix  multiplication  is  performed  for  each,  and  9in,jex  *s  c^osen 
that  leads  to  minimum  error.  Index  is  then  incremented  by  1  and  the 
process  repeated. 


V 


Description  of  INITIAL: 

This  subroutine  generates  a  curve  composed  of  segments  of  four 
circles,  and  then  generates  points  on  this  curve  corresponding  to 
joints  of  the  arm.  The  arm  is  then  made  to  follow  the  curve  by  various 
techniques. 
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Techniques  for  curve  matching. 

1)  0^  is  moved  so  as  to  put  position  of  joint  i+1  as  close 
as  possible  to  point  i+1  on  the  curve. 

2)  position  of  joint  i+1  and  slope  of  link  i  are  made  as 
close  as  possible  to  curve  at  corresponding  point  on  curve. 

3)  A  procedure  named  "match-ahead"  tries  to  match  (i+l)£il 
Joint  with  (i+l+A)  —  point  on  curve  (a  is  the  number  of 
joints  of  match-ahead)  by  moving  6^  • 

The  radius  of  the  four  circles  can  be  made  greater  than  or  equal  to 
the  smallest  radius  that  the  arm  can  turn. 

A3. 5  Description  of  Programs  -  Three-Dimensional  Model 

This  program  is  similiar  to  the  two-dimensional  case  with  the 
exception  of  changes  to  make  it  more  efficient. 

After  the  starting  configuration  and  amount  of  look-ahead  are 
specified,  three  matrices  are 

1)  the  identity  matrix 

2)  (B1  Bl+look) 

3)  (B2+look  *•*  Bn) 

where  "look"  is  the  amount  of 
Equation  A3-3.  Call  these  Mj^ 

Then  (M  )(M2>(M  )  is  the 
product  is  evaluated  for  the 
state  at  joint  1  chosen  which 


generated.  They  are 


look-ahead  and  is  defined  as  in 

,  m2  ,  and  m3  , 


total  transformation  matrix.  This 
look+1 

8  different  M2  matrices  and  the 


minimizes  the  error. 
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Then  Mj,  and 


M3  are  re-defined 


M1  =  Mi  X  Bx 

M3  ■  BMook  x  «3 

M2  '  <V . «B2+look> 

«nd  Is  chosen  In  a  manner  simlllar  to  that  of  01  •  Hie  process 

continues. 


n 
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APPENDIX  IV 


DETAILS  OF  CONFLICT  DETECTION 

The  methods  presented  here  result  directly  from  classical  vector 
geometry. 

A4 . 1  Objects  as  Spheres 

Note  that  the  physical  links  of  the  manipulator  are  modeled  as 
cylinders.  We  compute  the  distance  from  the  line  segment  that  is  the 
axis  of  this  cylinder  to  the  center  of  the  sphere.  The  problem  is  then 
to  find  the  distance  between  a  point  and  a  line  segment.  Consider  the 
line 

r  =  a  +  bt  (A4- 1) 

and  the  point  P,  described  by  the  vector  P  as  shown  in  Figure  A4.1. 


If  a  is  a  vector  to  the  end  of  the  line  segment,  b  a  unit  vector 
parallel  to  the  line,  t^  the  length  of  the  line  segment  and 

0<  t  <  t^ 

then  is  the  locus  of  points  on  the  line  segment.  A  vector,  d  ,  from 
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the  point  to  the  line,  normal  to  the  line  Is  given  by 


=  b  x  [(£-£.''  xb] 
=  (a-£)  -b  •  (£-£_)  b 


(A4-2) 


To  see  If  this  normal  cuts  the  segment  of  Interest,  we  note  fran 
Figure  A4.1  and  equation  (A4-1) 

£  =  a  +  bt*  «  £  +  d  (A4-3) 

where  t*  Is  the  value  of  t  where  the  normal  Intersects  the  line, 
then  from  (A4-3) 

t*  •=  b  •  [d  -  (a-£)J  (A4-4) 

Then  If 


0  <  t*  <  t. 

1  > 

the  normal  Intersects  the  segment.  If  this  Is  not  the  case,  we  find 
the  distance  between  P  and  the  end  points  of  the  line  segment  and 
choose  the  minimum.  A  routine  called  PTLINE  performs  these  calculations.. 


A4.2  Objects  as  Infinite  Planes 

Here  we  find  the  distances  between  the  end  points  of  a  line  segment 
and  a  plane.  Consider  the  plane  described  by  b  ,  a  unit  vector  normal 
to  the  plane  and  p  ,  the  distance  of  the  plane  from  the  origin  measured 
in  the  b  -  direction.  If  r  describes  a  point,  then  a  vector  from 
the  plane  to  the  point,  normal  to  the  plane  is  given  by,  d  : 

d  *  (r-b  -  p)b  (A4-5) 

A  routine  PLLINE  performs  the  computations. 


A4.3  Objects  as  Cylinders 

The  problem  is  to  find  the  shortest  distance  between  two  line 
segments,  which  are  the  axes  of  the  cylinders.  Consider  line  1  and 
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line  2  given  by: 


(A4-6) 

(AW) 


-i  =  — i  +  h  ci 
r 2  a  2  ^2  c2 

as  shown  in  Figure  A4.2, 

If  respectively:  £j  ,  £2  are  vectors  to  the  end  of  the  line 
segments,  ,  b2  unit  vectors  parallel  to  the  lines,  t^Q  ,  t£Q 
the  length  of  the  line  segments  and 


0  £  S  ho 
°ih^  ho 


then  ,  £2  are  the  loci  of  ooints  on  the  line  segments.  A  vector, 
d.  ,  from  line  2  to  line  l, 


normal  to  both  is  given  by 


±  -  (ai-az)-^^)  (b  xb2) 

i-(brk2)2 


(A4-8) 


Now  tc  find  where  the  normal  cuts  the  lines  we  find  where  line  2 
pierces  the  plane  containing  line  1  and  d.  .  The  locus  of  points,  £  , 
in  this  plane  may  be  written 

(I  '  ai)  *  [j2ix(]>ixb2)]  “  0  (A4-9) 
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This  is  of  the  form: 


£l  =  E2  +  -  •  (A4-12) 

We  next  determine  if  and  lie  on  the  segments  of  interest 

in  a  manner  similar  to  that  used  in  A4.1.  If  this  is  not  the  case 


then  ve  check  the  distance  between  endpoints  oi  one  line  segment  to 
the  other  line  segment  and  vice  versa  with  the  methods  of  A4.1.  A 
routine  LNLINE  performs  these  computations.  It  uses  PTLINE  if  necessary. 


APPENDIX  V 


SOLUTION  OF  A  6R,a  a  a  a  MANIPULATOR  AS  FOUR  QUADRATICS 


In  section  3.3.8  we  showed  how  the  solution  of  a  6R,aj323^ 
manipulator  could  be  reduced  to  a  single  polynomial  of  degree  eight. 

We  now  present  the  details  of  how  a  6R,aja2a4a3  manipulator  (with 
adjacent  axes  orthogonal)  can  be  expressed  as  four  quadratic  equations 
in  four  unknowns.  As  in  section  3.3.8,  we  w  ite  equations  for  the 
coordinate  (x,  y,  z)  of  the  point  of  intersection  of  axes  3  and  4 
(see  Figure  3.10). 

To  Include  a,.  ^  0  ,  we  use  equation  (3.100)  with  a^  ^  0  ,  a,.  j*  0 
a4  *  -90°  ,  a5  -  90°  ,  s2  =  s3  =  s4  -  s5  =  0  ,  to  obtain: 


-a4  c95  cfl6  -a5  ce6 

*  a4  ce5  s% 

’a4  S% 

1 

mm 

Similarly  (3.104),  and  (3.111)  become  respectively: 
2  2 

a4  +  a5  +  2a4  a5  c05 


(A5- 1) 


(A5-2) 


and 


Q  +  R  -  2^  •  R  "(“4  T  “5 


(a,  +  a 

7, 


5>  2  Ua5  [W 


-1 

+  a„  v  +  a  z  +  a 
237  33  14 


L- 

► 


-  2  2 
^=4a. ac 
4  5 


Then  from  the  components  of  P  in  (A 5-1)  we  find: 
7 

<=06  '  — 


ve5  +a5 


so  =  _Y_ 


a4ce5+a5 


s9  a-— 

D  a. 


(A5-3) 

(A5-4) 

(A5-5) 

(A5-6) 
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and  from  (3.104): 


c6, 


7  2  2  2 

P_  "a4"a5 

2a4a5 


(A5-7) 


Then  equation  (3.122)  may  be  written  with  c03,  s03,  s0^  ,  and  c0^ 
from  (A5-4)  -  (A5-7)  which  results  in: 


7r„.  1  1 


L-^]  Za4(a^c05+a5) 


-2  7x  \ 

-2  7y  \ 

7  2  7  2  2 

-2  z  +  p  +  a 

—  4 


(A5-8) 


L  0 

To  express  u>  4  in  system  1  we  use  (3.132)  and  (A5-8)  to  obtain: 

17  7  7  7  7??? 

-2  z  (an  x+a12  y+a13  z)+a13(  P  +a^-a5) 


~  4 


2a4(a4c05+a5) 


-2  *  ^a217x+a227y+a237z)+a23(7z2+Va5) 

7  7  7  7  7222 

-2  z  (a31  x+a32  y+a33  z)+a33(  P  +a4-a5> 


(A5-9) 


7  2 


Making  use  of  (3.109)  for  'P  and  (3.110)  for  x 
(A5-9)  becomes  after  simplification: 

-2(x-x4) (a13x+a23y+a33z+a"4) 

(+a13(W+R2-2xx4-2yy4-2zz4+a4-a3y 

/  -1 

'■2(y-y4)  (a13x+a23y+a33Z+a34) 

2  2  2 
ha23(W+R  -2xx4-2yy4-2zz4+a4-a5)y 


7  7  . 

“  »  y  ,  and 


—  4  = 


2a4(a4c05+a5) 


/- 2(z-z  )(a  x+a  y+a  z+a"*) 

4  13  237  33  34 

\jfa33(W+R2-2xx4-2yy^-2zz4+a4-a2)/ 


where  the  dot  product  in  (3.109)  has  been  evaluated,  R  has  been  expres- 

2  2? 

sed  in  terms  of  its  components  (x4  ,  y4  ,  z4)  ,  and  (x  +  y  +  z  )  has 


(A5-10) 
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been  replaced  by  the  new  variable  W  (equation  (3.141).  We  now  may 


form  (3.112)  with  =  90°.  We  use  (3.129)  for  and  (A5-10)  for 

,  and  obtain  after  dividing  by  common  factors  and  simplifying: 

-1  f  2  2  2  2  2“l 

0  *  2z(a13x+a23y+a33z+a34)  [-&  -a^+a5-a2+a1  I 

2  2  f  2  2  2 

+(W+a2-a1)|  a33(W+R  -2xx4-2yy4-2zz4+a4-a5) 

-ll  (A5-11) 

+2z^(aj3X+a23y+a33Z+a3^)J 

-12  2  2 
+2za3^(W+R_  -2xx4-2yy4-2zz4+a4-a5) 


Thrn  writing  (A5-3)  with  Q  and  R  expressed  in  terms  of  their  compon¬ 
ents  (x,  y,  z)  and  (x^  ,  y^  ,  z^)  respectively,  along  with  the 
definition  (3.141),  yields: 

|w+R2- ( 2xx4+2yy4+2zz4) - (a2+a2)J 


2  r  -12  21 

+4*5  (a13x+a23y+a33z+a14)  -a4  -  0 


(A5-12) 


Similarly  (3.138)  becomes  after  introducing  W  from  (3.141): 

(W-a2-a2)2+4a2(z2-a2)  -  0  .  (A5-13) 

The  equations  (A5-11),  (A5-12),  (A-13),  and  (3.141)  are  the  four 
quadratics  in  the  unknowns  x,  y,  z,  and  W  . 
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